From 3010f313dc2e291d3b2f1f6022f132ce227b1895 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Mar 2015 09:04:01 +0100 Subject: [PATCH 001/493] Fix IO update when safety can not be set to on. From @zottgrammes Conflicts: ROMFS/px4fmu_common/init.d/rcS --- ROMFS/px4fmu_common/init.d/rcS | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 580043a1d4..816b1152fe 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -198,8 +198,17 @@ then tone_alarm MLL32CP8MB - px4io start - px4io safety_on + if px4io start + then + # try to safe px4 io so motor outputs dont go crazy + if px4io safety_on + then + # success! no-op + else + # px4io did not respond to the safety command + px4io stop + fi + fi if px4io forceupdate 14662 ${IO_FILE} then From 79e084a154c3b4d945fb00d85fc2307667f9ae2f Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 26 May 2015 18:05:26 +0100 Subject: [PATCH 002/493] drivers: added validity check --- src/drivers/ll40ls/ll40ls.cpp | 2 ++ src/drivers/mb12xx/mb12xx.cpp | 4 ++++ src/drivers/trone/trone.cpp | 10 ++++++++-- 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8984220625..d806638788 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -295,6 +295,8 @@ test(const bool use_i2c, const int bus) } warnx("periodic read %u", i); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index afeb8e5545..ec07399f54 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -832,6 +832,7 @@ test() } warnx("single read"); + warnx("measurement: %0.2f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); /* start the sensor polling at 2Hz */ @@ -860,6 +861,9 @@ test() } warnx("periodic read %u", i); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f", (double)report.current_distance); warnx("time: %llu", report.timestamp); } diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 23e52547a1..6150cc90e2 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -125,6 +125,7 @@ private: work_s _work; ringbuffer::RingBuffer *_reports; bool _sensor_ok; + uint8_t _valid; int _measure_ticks; bool _collect_phase; int _class_instance; @@ -211,7 +212,7 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -/* static uint8_t crc8(uint8_t *p, uint8_t len) { + static uint8_t crc8(uint8_t *p, uint8_t len) { uint16_t i; uint16_t crc = 0x0; @@ -221,7 +222,7 @@ static const uint8_t crc_table[] = { } return crc & 0xFF; -}*/ +} /* * Driver 'main' command. @@ -234,6 +235,7 @@ TRONE::TRONE(int bus, int address) : _max_distance(TRONE_MAX_DISTANCE), _reports(nullptr), _sensor_ok(false), + _valid(0), _measure_ticks(0), _collect_phase(false), _class_instance(-1), @@ -586,6 +588,10 @@ TRONE::collect() /* TODO: set proper ID */ report.id = 0; + // This validation check can be used later + _valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0; + /* publish it, if we are the primary */ if (_distance_sensor_topic >= 0) { orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); From ff1f3ba7f1f8ca52a2e4cddac4d7436b8ccc8936 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 26 May 2015 18:08:55 +0100 Subject: [PATCH 003/493] drivers: added validity check to sf0x --- src/drivers/sf0x/sf0x.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 431184d046..7861c0596b 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -854,7 +854,7 @@ test() } warnx("single read"); - warnx("val: %0.2f m", (double)report.current_distance); + warnx("measurement: %0.2f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); /* start the sensor polling at 2 Hz rate */ @@ -885,7 +885,9 @@ test() } warnx("read #%u", i); - warnx("val: %0.3f m", (double)report.current_distance); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); } From adbccfaa1cd100609490b61f2081a0619b0a36c8 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 3 Jun 2015 09:32:02 -0400 Subject: [PATCH 004/493] Saturate velocity command for mc_pos_control. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 7a3a5a679b..2509f2b8c4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1032,7 +1032,14 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; + /* make sure velocity setpoint is saturated */ _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + for (int i=0; i<3; i++) { + if (_vel_sp(i) > _params.vel_max(i)) { + _vel_sp(i) = _params.vel_max(i); + } else if (_vel_sp(i) < -_params.vel_max(i)) + _vel_sp(i) = -_params.vel_max(i); + } if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; From dedd16e36e4f0690f8662b93f2aa8144cc8a57bf Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 3 Jun 2015 21:15:17 -0400 Subject: [PATCH 005/493] Modified velocity saturation to maintain direction. --- .../mc_pos_control/mc_pos_control_main.cpp | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2509f2b8c4..995937aa6c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1032,13 +1032,21 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; - /* make sure velocity setpoint is saturated */ _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; - for (int i=0; i<3; i++) { - if (_vel_sp(i) > _params.vel_max(i)) { - _vel_sp(i) = _params.vel_max(i); - } else if (_vel_sp(i) < -_params.vel_max(i)) - _vel_sp(i) = -_params.vel_max(i); + + /* make sure velocity setpoint is saturated in xy*/ + float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + + _vel_sp(1)*_vel_sp(1)); + if (vel_norm_xy > _params.vel_max(0)) { + /* note assumes vel_max(0) == vel_max(1) */ + _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; + _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + } + + /* make sure velocity setpoint is saturated in z*/ + float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { + _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; } if (!_control_mode.flag_control_altitude_enabled) { From 0bfc727584d09bd910129ce3c03551b5ec2a5b35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 13:30:44 +0200 Subject: [PATCH 006/493] Add more functionality to HIL driver --- src/drivers/hil/hil.cpp | 126 +++++++++++++++++++++++++++++++--------- 1 file changed, 97 insertions(+), 29 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index a7c2e83e34..0fbabaf2f1 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -264,36 +264,42 @@ HIL::set_mode(Mode mode) debug("MODE_2PWM"); /* multi-port with flow control lines as PWM */ _update_rate = 50; /* default output rate */ + _num_outputs = 2; break; case MODE_4PWM: debug("MODE_4PWM"); /* multi-port as 4 PWM outs */ _update_rate = 50; /* default output rate */ + _num_outputs = 4; break; case MODE_8PWM: - debug("MODE_8PWM"); - /* multi-port as 8 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + debug("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 8; + break; - case MODE_12PWM: - debug("MODE_12PWM"); - /* multi-port as 12 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + case MODE_12PWM: + debug("MODE_12PWM"); + /* multi-port as 12 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 12; + break; - case MODE_16PWM: - debug("MODE_16PWM"); - /* multi-port as 16 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + case MODE_16PWM: + debug("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 16; + break; case MODE_NONE: debug("MODE_NONE"); /* disable servo outputs and set a very low update rate */ _update_rate = 10; + _num_outputs = 0; break; default: @@ -468,13 +474,6 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - debug("ioctl 0x%04x 0x%08x", cmd, arg); - - // /* try it as a GPIO ioctl first */ - // ret = HIL::gpio_ioctl(filp, cmd, arg); - // if (ret != -ENOTTY) - // return ret; - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ switch(_mode) { case MODE_2PWM: @@ -523,6 +522,62 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) // HIL always outputs at the alternate (usually faster) rate break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + *(uint32_t *)arg = 400; + break; + + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = 400; + break; + + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + *(uint32_t *)arg = 0; + break; + + case PWM_SERVO_GET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 850; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 900; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 1000; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 2000; + } + + pwm->channel_count = _num_outputs; + break; + } + case PWM_SERVO_SET(2): case PWM_SERVO_SET(3): if (_mode != MODE_4PWM) { @@ -543,18 +598,26 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; - case PWM_SERVO_GET(2): + case PWM_SERVO_GET(7): + case PWM_SERVO_GET(6): + case PWM_SERVO_GET(5): + case PWM_SERVO_GET(4): + if (_num_outputs < 8) { + ret = -EINVAL; + break; + } + case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_GET(2): + if (_num_outputs < 4) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_GET(0): - case PWM_SERVO_GET(1): { - // channel = cmd - PWM_SERVO_SET(0); - // *(servo_position_t *)arg = up_pwm_servo_get(channel); + case PWM_SERVO_GET(1): + case PWM_SERVO_GET(0): { + *(servo_position_t *)arg = 1500; break; } @@ -566,11 +629,16 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } + case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; + if (_mode == MODE_8PWM) { + *(unsigned *)arg = 8; + } else if (_mode == MODE_4PWM) { + + *(unsigned *)arg = 4; } else { + *(unsigned *)arg = 2; } From 6c0539c243a13ed0cb2c4c508b4770bdff57add6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:55:55 +0200 Subject: [PATCH 007/493] FW position controller: Do handle idle mission items correctly --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b5861d0f16..90cf391536 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1085,7 +1085,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust = 0.0f; + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); @@ -1544,6 +1549,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust = 0.0f; } else { /* Copy thrust and pitch values from tecs */ _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : From 05993bee6fa523d6d8ecfcceb614fa45fe669956 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:57:27 +0200 Subject: [PATCH 008/493] Navigator: Provide better feedback if no mission present, enforce minimum altitude in loiter and in auto modes --- src/modules/navigator/loiter.cpp | 5 ++-- src/modules/navigator/loiter.h | 3 +++ src/modules/navigator/mission.cpp | 32 ++++++++++++++++++------- src/modules/navigator/mission_block.cpp | 8 +++++-- src/modules/navigator/mission_block.h | 2 +- 5 files changed, 37 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index a744d58cf0..aabdb2b075 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -55,7 +55,8 @@ #include "navigator.h" Loiter::Loiter(Navigator *navigator, const char *name) : - MissionBlock(navigator, name) + MissionBlock(navigator, name), + _param_min_alt(this, "MIS_TAKEOFF_ALT", false) { /* load initial params */ updateParams(); @@ -74,7 +75,7 @@ void Loiter::on_activation() { /* set current mission item to loiter */ - set_loiter_item(&_mission_item); + set_loiter_item(&_mission_item, _param_min_alt.get()); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 37ab57a078..0627c54129 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -59,6 +59,9 @@ public: virtual void on_activation(); virtual void on_active(); + +private: + control::BlockParamFloat _param_min_alt; }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fe876ee8b1..a74e042a91 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -377,6 +377,7 @@ Mission::set_mission_items() /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); + user_feedback_done = true; } _mission_type = MISSION_TYPE_ONBOARD; @@ -385,6 +386,7 @@ Mission::set_mission_items() /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); + user_feedback_done = true; } _mission_type = MISSION_TYPE_OFFBOARD; } else { @@ -392,21 +394,17 @@ Mission::set_mission_items() if (_mission_type != MISSION_TYPE_NONE) { /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + user_feedback_done = true; /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); - } else if (!user_feedback_done) { - /* only tell users that we got no mission if there has not been any - * better, more specific feedback yet - * https://en.wikipedia.org/wiki/Loiter_(aeronautics) - */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); } + _mission_type = MISSION_TYPE_NONE; - /* set loiter mission item */ - set_loiter_item(&_mission_item); + /* set loiter mission item and ensure that there is a minimum clearance from home */ + set_loiter_item(&_mission_item, _param_takeoff_alt.get()); /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; @@ -418,6 +416,24 @@ Mission::set_mission_items() set_mission_finished(); + if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + * https://en.wikipedia.org/wiki/Loiter_(aeronautics) + */ + + if (_navigator->get_vstatus()->condition_landed) { + /* landed, refusing to take off without a mission */ + + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff"); + } else { + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); + } + + user_feedback_done = true; + + } + _navigator->set_position_setpoint_triplet_updated(); return; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 42c74428ad..8e83a3329d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -228,7 +228,7 @@ MissionBlock::set_previous_pos_setpoint() } void -MissionBlock::set_loiter_item(struct mission_item_s *item) +MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) { if (_navigator->get_vstatus()->condition_landed) { /* landed, don't takeoff, but switch to IDLE mode */ @@ -246,10 +246,14 @@ MissionBlock::set_loiter_item(struct mission_item_s *item) item->altitude = pos_sp_triplet->current.alt; } else { - /* use current position */ + /* use current position and use return altitude as clearance */ item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; item->altitude = _navigator->get_global_position()->alt; + + if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) { + item->altitude = _navigator->get_home_position()->alt + min_clearance; + } } item->altitude_is_relative = false; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index ec3e305825..4e6e99acb0 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -91,7 +91,7 @@ protected: /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - void set_loiter_item(struct mission_item_s *item); + void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f); mission_item_s _mission_item; bool _waypoint_position_reached; From 92aeef2b846661a9b6f41347ea29ae1e0bb2e48b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:57:57 +0200 Subject: [PATCH 009/493] commander: Better text feedback --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7a379612da..e975cf87ef 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -375,7 +375,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, switch (new_state) { case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); + mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; From 3f77455dd85870caacc5dfa76aecd669a43de2e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:58:21 +0200 Subject: [PATCH 010/493] commander: Condition HIL arming check properly --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7c15992f40..67e17aae08 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -450,7 +450,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char transition_result_t arming_res = TRANSITION_NOT_CHANGED; // For HIL platforms, require that simulated sensors are connected - if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL && + is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); return TRANSITION_DENIED; } From 8838b18da75d6f4354f73b38152c2ca98f9197aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 18:53:38 +0200 Subject: [PATCH 011/493] FW attitude control: Run attitude controller as fast as we can to minimize latency --- src/modules/fw_att_control/fw_att_control_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c44f29a404..fe27de14f5 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -634,8 +634,9 @@ FixedwingAttitudeControl::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */ - orb_set_interval(_att_sub, 17); + /* do not limit the attitude updates in order to minimize latency. + * actuator outputs are still limited by the individual drivers + * properly to not saturate IO or physical limitations */ parameters_update(); From 55ed9e96126cab150dbad1d9bd9db392b75781d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 18:54:28 +0200 Subject: [PATCH 012/493] ECL: Run TECS filter faster, adjust gains accordingly --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index cfcc48b62a..d13673ec9b 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -89,7 +89,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 // take 5 point moving average //_vel_dot = _vdot_filter.apply(temp); // XXX resolve this properly - _vel_dot = 0.9f * _vel_dot + 0.1f * temp; + _vel_dot = 0.95f * _vel_dot + 0.05f * temp; } else { _vel_dot = 0.0f; From f9f34078d15281f3edfc0a1e0d49ee1676ee2d33 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 00:16:25 +0200 Subject: [PATCH 013/493] commander: Ensure RTL can be triggered in all modes --- src/modules/commander/commander.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7c15992f40..f6780e2af1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2504,12 +2504,15 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + /* override is not ok for the RTL and recovery mode */ + control_mode.flag_external_manual_override_ok = false; + /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; From dccd4df7bcd061cb38e9eca2a539d838cb5af1a9 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 13 Jun 2015 17:03:31 +0100 Subject: [PATCH 014/493] mocap_support: added support for mocap data on firmware --- msg/att_pos_mocap.msg | 10 ++ msg/vehicle_vicon_position.msg | 10 -- .../attitude_estimator_ekf_main.cpp | 26 ++++- src/modules/mavlink/mavlink_messages.cpp | 53 ++++++----- src/modules/mavlink/mavlink_receiver.cpp | 43 +++++---- src/modules/mavlink/mavlink_receiver.h | 6 +- src/modules/position_estimator_inav/module.mk | 2 +- .../position_estimator_inav_main.c | 95 ++++++++++++++++++- .../position_estimator_inav_params.c | 14 +++ .../position_estimator_inav_params.h | 2 + src/modules/sdlog2/sdlog2.c | 31 +++--- src/modules/sdlog2/sdlog2_messages.h | 19 ++-- src/modules/uORB/objects_common.cpp | 4 +- 13 files changed, 225 insertions(+), 90 deletions(-) create mode 100644 msg/att_pos_mocap.msg delete mode 100644 msg/vehicle_vicon_position.msg diff --git a/msg/att_pos_mocap.msg b/msg/att_pos_mocap.msg new file mode 100644 index 0000000000..52bc04b5aa --- /dev/null +++ b/msg/att_pos_mocap.msg @@ -0,0 +1,10 @@ +uint32 id # ID of the estimator, commonly the component ID of the incoming message + +uint64 timestamp_boot # time of this estimate, in microseconds since system start +uint64 timestamp_computer # timestamp provided by the companion computer, in us + +float32[4] q # Estimated attitude as quaternion + +float32 x # X position in meters in NED earth-fixed frame +float32 y # Y position in meters in NED earth-fixed frame +float32 z # Z position in meters in NED earth-fixed frame (negative altitude) diff --git a/msg/vehicle_vicon_position.msg b/msg/vehicle_vicon_position.msg deleted file mode 100644 index 1626d85383..0000000000 --- a/msg/vehicle_vicon_position.msg +++ /dev/null @@ -1,10 +0,0 @@ -uint64 timestamp # time of this estimate, in microseconds since system start -bool valid # true if position satisfies validity criteria of estimator - -float32 x # X position in meters in NED earth-fixed frame -float32 y # Y position in meters in NED earth-fixed frame -float32 z # Z position in meters in NED earth-fixed frame (negative altitude) -float32 roll -float32 pitch -float32 yaw -float32[4] q # Attitude as quaternion diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index afd8706dbf..a320ffe518 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include @@ -261,6 +262,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to vision estimate */ int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + /* subscribe to mocap data */ + int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -291,6 +295,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) R_decl.identity(); struct vision_position_estimate_s vision {}; + struct att_pos_mocap_s mocap {}; /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); @@ -445,11 +450,30 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) bool vision_updated = false; orb_check(vision_sub, &vision_updated); + bool mocap_updated = false; + orb_check(mocap_sub, &mocap_updated); + if (vision_updated) { orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision); } - if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { + if (mocap_updated) { + orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap); + } + + if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) { + + math::Quaternion q(mocap.q); + math::Matrix<3, 3> Rmoc = q.to_dcm(); + + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + z_k[6] = vn(0); + z_k[7] = vn(1); + z_k[8] = vn(2); + }else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { math::Quaternion q(vision.q); math::Matrix<3, 3> Rvis = q.to_dcm(); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1b2689e6bb..1c157c6e84 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -54,7 +54,7 @@ #include #include #include -#include +#include #include #include #include @@ -1187,64 +1187,65 @@ protected: }; -class MavlinkStreamViconPositionEstimate : public MavlinkStream +class MavlinkStreamAttPosMocap : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamViconPositionEstimate::get_name_static(); + return MavlinkStreamAttPosMocap::get_name_static(); } static const char *get_name_static() { - return "VICON_POSITION_ESTIMATE"; + return "ATT_POS_MOCAP"; } uint8_t get_id() { - return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + return MAVLINK_MSG_ID_ATT_POS_MOCAP; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamViconPositionEstimate(mavlink); + return new MavlinkStreamAttPosMocap(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *_pos_sub; - uint64_t _pos_time; + MavlinkOrbSubscription *_mocap_sub; + uint64_t _mocap_time; /* do not allow top copying this class */ - MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); - MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); + MavlinkStreamAttPosMocap(MavlinkStreamAttPosMocap &); + MavlinkStreamAttPosMocap& operator = (const MavlinkStreamAttPosMocap &); protected: - explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), - _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))), - _pos_time(0) + explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink), + _mocap_sub(_mavlink->add_orb_subscription(ORB_ID(att_pos_mocap))), + _mocap_time(0) {} void send(const hrt_abstime t) { - struct vehicle_vicon_position_s pos; + struct att_pos_mocap_s mocap; - if (_pos_sub->update(&_pos_time, &pos)) { - mavlink_vicon_position_estimate_t msg; + if (_mocap_sub->update(&_mocap_time, &mocap)) { + mavlink_att_pos_mocap_t msg; - msg.usec = pos.timestamp; - msg.x = pos.x; - msg.y = pos.y; - msg.z = pos.z; - msg.roll = pos.roll; - msg.pitch = pos.pitch; - msg.yaw = pos.yaw; + msg.time_usec = mocap.timestamp_boot; + msg.q[0] = mocap.q[0]; + msg.q[1] = mocap.q[1]; + msg.q[2] = mocap.q[2]; + msg.q[3] = mocap.q[3]; + msg.x = mocap.x; + msg.y = mocap.y; + msg.z = mocap.z; - _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, &msg); } } }; @@ -2277,7 +2278,7 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), - new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), + new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static), new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4e3da6c7c0..575321c4d0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -118,7 +118,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _rates_sp_pub(nullptr), _force_sp_pub(nullptr), _pos_sp_triplet_pub(nullptr), - _vicon_position_pub(nullptr), + _att_pos_mocap_pub(nullptr), _vision_position_pub(nullptr), _telemetry_status_pub(nullptr), _rc_pub(nullptr), @@ -169,8 +169,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_set_mode(msg); break; - case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: - handle_message_vicon_position_estimate(msg); + case MAVLINK_MSG_ID_ATT_POS_MOCAP: + handle_message_att_pos_mocap(msg); break; case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: @@ -556,27 +556,34 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) +MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) { - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(msg, &pos); + mavlink_att_pos_mocap_t mocap; + mavlink_msg_att_pos_mocap_decode(msg, &mocap); - struct vehicle_vicon_position_s vicon_position; - memset(&vicon_position, 0, sizeof(vicon_position)); + struct att_pos_mocap_s att_pos_mocap; + memset(&att_pos_mocap, 0, sizeof(att_pos_mocap)); - vicon_position.timestamp = hrt_absolute_time(); - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - vicon_position.roll = pos.roll; - vicon_position.pitch = pos.pitch; - vicon_position.yaw = pos.yaw; + // Use the component ID to identify the mocap system + att_pos_mocap.id = msg->compid; - if (_vicon_position_pub == nullptr) { - _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + att_pos_mocap.timestamp_boot = hrt_absolute_time(); // Monotonic time + att_pos_mocap.timestamp_computer = sync_stamp(mocap.time_usec); // Synced time + + att_pos_mocap.q[0] = mocap.q[0]; + att_pos_mocap.q[1] = mocap.q[1]; + att_pos_mocap.q[2] = mocap.q[2]; + att_pos_mocap.q[3] = mocap.q[3]; + + att_pos_mocap.x = mocap.x; + att_pos_mocap.y = mocap.y; + att_pos_mocap.z = mocap.z; + + if (_att_pos_mocap_pub == nullptr) { + _att_pos_mocap_pub = orb_advertise(ORB_ID(att_pos_mocap), &att_pos_mocap); } else { - orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); + orb_publish(ORB_ID(att_pos_mocap), _att_pos_mocap_pub, &att_pos_mocap); } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8fffad4c3e..2709a10915 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include #include @@ -119,7 +119,7 @@ private: void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); - void handle_message_vicon_position_estimate(mavlink_message_t *msg); + void handle_message_att_pos_mocap(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); @@ -174,7 +174,7 @@ private: orb_advert_t _rates_sp_pub; orb_advert_t _force_sp_pub; orb_advert_t _pos_sp_triplet_pub; - orb_advert_t _vicon_position_pub; + orb_advert_t _att_pos_mocap_pub; orb_advert_t _vision_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 45c8762996..56aa3fad04 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -42,5 +42,5 @@ SRCS = position_estimator_inav_main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wframe-larger-than=3500 +EXTRACFLAGS = -Wframe-larger-than=3800 diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 8654a7cb11..eaad4e3156 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -36,6 +36,7 @@ * Model-identification based position estimator for multirotors * * @author Anton Babushkin + * @author Nuno Marques */ #include @@ -64,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -87,6 +89,7 @@ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s +static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms @@ -241,6 +244,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float eph_vision = 0.2f; float epv_vision = 0.2f; + float eph_mocap = 0.05f; + float epv_mocap = 0.05f; + float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -267,6 +273,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint16_t gps_updates = 0; uint16_t attitude_updates = 0; uint16_t flow_updates = 0; + uint16_t vision_updates = 0; + uint16_t mocap_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); hrt_abstime pub_last = hrt_absolute_time(); @@ -291,6 +299,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { 0.0f, 0.0f }, // D (pos, vel) }; + float corr_mocap[3][1] = { + { 0.0f }, // N (pos) + { 0.0f }, // E (pos) + { 0.0f }, // D (pos) + }; + float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; @@ -306,7 +320,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool sonar_valid = false; // sonar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) - bool vision_valid = false; + bool vision_valid = false; // vision is valid + bool mocap_valid = false; // mocap is valid /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -327,6 +342,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&flow, 0, sizeof(flow)); struct vision_position_estimate_s vision; memset(&vision, 0, sizeof(vision)); + struct att_pos_mocap_s mocap; + memset(&mocap, 0, sizeof(mocap)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); @@ -339,6 +356,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ @@ -699,9 +717,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_vision[2][1] = 0.0f - z_est[1]; } + vision_updates++; } } + /* vehicle mocap position */ + orb_check(att_pos_mocap_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap); + + /* reset position estimate on first mocap update */ + if (!mocap_valid) { + x_est[0] = mocap.x; + y_est[0] = mocap.y; + z_est[0] = mocap.z; + + mocap_valid = true; + + warnx("MOCAP data valid"); + mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid"); + } + + /* calculate correction for position */ + corr_mocap[0][0] = mocap.x - x_est[0]; + corr_mocap[1][0] = mocap.y - y_est[0]; + corr_mocap[2][0] = mocap.z - z_est[0]; + + mocap_updates++; + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); @@ -832,6 +877,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); } + /* check for timeout on mocap topic */ + if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) { + mocap_valid = false; + warnx("MOCAP timeout"); + mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout"); + } + /* check for sonar measurement timeout */ if (sonar_valid && (t > (sonar_time + sonar_timeout))) { corr_sonar = 0.0f; @@ -856,10 +908,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* use VISION if it's valid and has a valid weight parameter */ bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; + /* use MOCAP if it's valid and has a valid weight parameter */ + bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy; + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -883,6 +937,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_vision_v = params.w_xy_vision_v; float w_z_vision_p = params.w_z_vision_p; + float w_mocap_p = params.w_mocap_p; + /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; @@ -940,6 +996,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; } + /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */ + accel_bias_corr[0] = 0.0f; + accel_bias_corr[1] = 0.0f; + accel_bias_corr[2] = 0.0f; + + if (use_mocap) { + accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p; + accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p; + accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p; + } + /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; @@ -1001,11 +1068,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); } + if (use_mocap) { + epv = fminf(epv, epv_mocap); + inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); + } + if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); + memset(corr_mocap, 0, sizeof(corr_mocap)); corr_baro = 0; } else { @@ -1055,12 +1128,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + if (use_mocap) { + eph = fminf(eph, eph_mocap); + + inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p); + inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); + } + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); + memset(corr_mocap, 0, sizeof(corr_mocap)); memset(corr_flow, 0, sizeof(corr_flow)); } else { @@ -1078,18 +1159,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( - "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", + "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s", (double)(accel_updates / updates_dt), (double)(baro_updates / updates_dt), (double)(gps_updates / updates_dt), (double)(attitude_updates / updates_dt), - (double)(flow_updates / updates_dt)); + (double)(flow_updates / updates_dt), + (double)(vision_updates / updates_dt), + (double)(mocap_updates / updates_dt)); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; flow_updates = 0; + vision_updates = 0; + mocap_updates = 0; } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 382e9e46d7..a9ddafc0de 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -140,6 +140,18 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f); */ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); +/** + * Weight for mocap system + * + * Weight (cutoff frequency) for mocap position measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ + + PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f); + /** * XY axis weight for optical flow * @@ -312,6 +324,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V"); + h->w_mocap_p = param_find("INAV_W_MOC_P"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); @@ -339,6 +352,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); param_get(h->w_xy_vision_v, &(p->w_xy_vision_v)); + param_get(h->w_mocap_p, &(p->w_mocap_p)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 51bbda412a..d6cb9ed410 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -51,6 +51,7 @@ struct position_estimator_inav_params { float w_xy_gps_v; float w_xy_vision_p; float w_xy_vision_v; + float w_mocap_p; float w_xy_flow; float w_xy_res_v; float w_gps_flow; @@ -76,6 +77,7 @@ struct position_estimator_inav_param_handles { param_t w_xy_gps_v; param_t w_xy_vision_p; param_t w_xy_vision_v; + param_t w_mocap_p; param_t w_xy_flow; param_t w_xy_res_v; param_t w_gps_flow; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index acc4771968..ef50fe7e2d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -80,7 +80,7 @@ #include #include #include -#include +#include #include #include #include @@ -1071,7 +1071,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; - struct vehicle_vicon_position_s vicon_pos; + struct att_pos_mocap_s att_pos_mocap; struct vision_position_estimate_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; @@ -1127,7 +1127,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST0_s log_EST0; struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; - struct log_VICN_s log_VICN; + struct log_MOCP_s log_MOCP; struct log_VISN_s log_VISN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; @@ -1162,7 +1162,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int triplet_sub; int gps_pos_sub; int sat_info_sub; - int vicon_pos_sub; + int att_pos_mocap_sub; int vision_pos_sub; int flow_sub; int rc_sub; @@ -1197,7 +1197,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.local_pos_sp_sub = -1; subs.global_pos_sub = -1; subs.triplet_sub = -1; - subs.vicon_pos_sub = -1; + subs.att_pos_mocap_sub = -1; subs.vision_pos_sub = -1; subs.flow_sub = -1; subs.rc_sub = -1; @@ -1681,16 +1681,17 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- VICON POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) { - log_msg.msg_type = LOG_VICN_MSG; - log_msg.body.log_VICN.x = buf.vicon_pos.x; - log_msg.body.log_VICN.y = buf.vicon_pos.y; - log_msg.body.log_VICN.z = buf.vicon_pos.z; - log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch; - log_msg.body.log_VICN.roll = buf.vicon_pos.roll; - log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; - LOGBUFFER_WRITE_AND_COUNT(VICN); + /* --- MOCAP ATTITUDE AND POSITION --- */ + if (copy_if_updated(ORB_ID(att_pos_mocap), &subs.att_pos_mocap_sub, &buf.att_pos_mocap)) { + log_msg.msg_type = LOG_MOCP_MSG; + log_msg.body.log_MOCP.qw = buf.att_pos_mocap.q[0]; + log_msg.body.log_MOCP.qx = buf.att_pos_mocap.q[1]; + log_msg.body.log_MOCP.qy = buf.att_pos_mocap.q[2]; + log_msg.body.log_MOCP.qz = buf.att_pos_mocap.q[3]; + log_msg.body.log_MOCP.x = buf.att_pos_mocap.x; + log_msg.body.log_MOCP.y = buf.att_pos_mocap.y; + log_msg.body.log_MOCP.z = buf.att_pos_mocap.z; + LOGBUFFER_WRITE_AND_COUNT(MOCP); } /* --- VISION POSITION --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9cf37683ae..f665eed2a2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -321,15 +321,16 @@ struct log_PWR_s { uint8_t high_power_rail_overcurrent; }; -/* --- VICN - VICON POSITION --- */ -#define LOG_VICN_MSG 25 -struct log_VICN_s { +/* --- MOCP - MOCAP ATTITUDE AND POSITION --- */ +#define LOG_MOCP_MSG 25 +struct log_MOCP_s { + float qw; + float qx; + float qy; + float qz; float x; float y; float z; - float roll; - float pitch; - float yaw; }; /* --- GS0A - GPS SNR #0, SAT GROUP A --- */ @@ -427,10 +428,10 @@ struct log_VISN_s { float vx; float vy; float vz; + float qw; float qx; float qy; float qz; - float qw; }; /* --- ENCODERS - ENCODER DATA --- */ @@ -523,8 +524,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), - LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), - LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"), + 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"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 9f980eebea..de628b3f86 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -108,8 +108,8 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); #include "topics/vehicle_local_position.h" ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s); -#include "topics/vehicle_vicon_position.h" -ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); +#include "topics/att_pos_mocap.h" +ORB_DEFINE(att_pos_mocap, struct att_pos_mocap_s); #include "topics/vehicle_rates_setpoint.h" ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); From 7deeda726cbeff7259dc671244990b8532146b99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 12:07:32 +0200 Subject: [PATCH 015/493] airspeed topic: Add unfiltered airspeed --- msg/airspeed.msg | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/msg/airspeed.msg b/msg/airspeed.msg index 8d6af2138d..525bfd7f88 100644 --- a/msg/airspeed.msg +++ b/msg/airspeed.msg @@ -1,4 +1,5 @@ uint64 timestamp # microseconds since system boot, needed to integrate float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown -float32 true_airspeed_m_s # true airspeed in meters per second, -1 if unknown +float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown +float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown From 0916e6fc199fee2acbefa924b55082eb483ef3bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 12:09:21 +0200 Subject: [PATCH 016/493] sensors app: Populate unfiltered airspeed field --- src/modules/sensors/sensors.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 203564ec97..1e831becd9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1288,6 +1288,10 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ From e76bdc3cace535108aa90ca89eadfbaef1f13b01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 12:10:36 +0200 Subject: [PATCH 017/493] EKF: Use unfiltered airspeed if airspeed is large enough - rely for better stability on the filtered speed for the threshold. Lower the threshold to 5 m/s to ensure airspeed fusion even on small wings --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index b9897ffcfc..84da033adb 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1062,7 +1062,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const } // Fuse Airspeed Measurements - if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) { + if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data @@ -1320,7 +1320,7 @@ void AttitudePositionEstimatorEKF::pollData() orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); - _ekf->VtasMeas = _airspeed.true_airspeed_m_s; + _ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s; } From 44441ab501be45165d4eeb2e0f138e2153f9f66e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 14:05:17 +0200 Subject: [PATCH 018/493] FW pos control: Perform climbout if user requests more than 85% pitch up --- .../fw_pos_control_l1_main.cpp | 45 +++++++++++-------- 1 file changed, 27 insertions(+), 18 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 90cf391536..01d94a8881 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -100,8 +100,8 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading -#define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes - +static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode /** * L1 control app start / stop handling function @@ -370,7 +370,7 @@ private: /** * Publish navigation capabilities */ - void navigation_capabilities_publish(); + void navigation_capabilities_publish(); /** * Get a new waypoint based on heading and distance from current position @@ -386,27 +386,30 @@ private: /** * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available */ - float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); + float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** * Control position. */ /** - * Do takeoff help when in altitude controlled modes + * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + void do_takeoff_help(); /** - * Update desired altitude base on user pitch stick input + * Update desired altitude base on user pitch stick input + * + * @param dt Time step + * @return true if climbout mode was requested by user (climb with max rate and min airspeed) */ - void update_desired_altitude(float dt); + bool update_desired_altitude(float dt); bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); - float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); + float calculate_target_airspeed(float airspeed_demand); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -421,12 +424,12 @@ private: /* * Reset takeoff state */ - void reset_takeoff_state(); + void reset_takeoff_state(); /* * Reset landing state */ - void reset_landing_state(); + void reset_landing_state(); /* * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) @@ -955,16 +958,20 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } -void FixedwingPositionControl::update_desired_altitude(float dt) +bool FixedwingPositionControl::update_desired_altitude(float dt) { const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; static bool was_in_deadband = false; + bool climbout_mode = false; + + // XXX the sign magic in this function needs to be fixed if (_manual.x > deadBand) { float pitch = (_manual.x - deadBand) / factor; _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; was_in_deadband = false; + climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH); } else if (_manual.x < - deadBand) { float pitch = (_manual.x + deadBand) / factor; _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; @@ -976,6 +983,8 @@ void FixedwingPositionControl::update_desired_altitude(float dt) _hold_alt = _global_pos.alt; was_in_deadband = true; } + + return climbout_mode; } void FixedwingPositionControl::do_takeoff_help() @@ -1275,7 +1284,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + mavlink_log_critical(_mavlink_fd, "Launchdetection running"); last_sent = hrt_absolute_time(); } @@ -1404,7 +1413,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _manual.z; /* update desired altitude based on user pitch stick input */ - update_desired_altitude(dt); + bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); @@ -1423,7 +1432,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, throttle_max, _parameters.throttle_cruise, - false, + climbout_requested, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, @@ -1497,7 +1506,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _manual.z; /* update desired altitude based on user pitch stick input */ - update_desired_altitude(dt); + bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); @@ -1516,7 +1525,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, throttle_max, _parameters.throttle_cruise, - false, + climbout_requested, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, From a8537b8818d8c7839548a93e9c479d0e45d80194 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 18 May 2015 08:20:18 +0530 Subject: [PATCH 019/493] camera trigger : initial import --- src/modules/camera_trigger/camera_trigger.cpp | 382 ++++++++++++++++++ .../camera_trigger/camera_trigger_params.c | 89 ++++ src/modules/camera_trigger/module.mk | 43 ++ 3 files changed, 514 insertions(+) create mode 100644 src/modules/camera_trigger/camera_trigger.cpp create mode 100644 src/modules/camera_trigger/camera_trigger_params.c create mode 100644 src/modules/camera_trigger/module.mk diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp new file mode 100644 index 0000000000..f0c86e42f2 --- /dev/null +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -0,0 +1,382 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Mohammed Kabir + * + * 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 camera_trigger.cpp + * + * External camera-IMU synchronisation and triggering via FMU auxillary pins. + * + * @author Mohammed Kabir + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); + +class CameraTrigger +{ +public: + /** + * Constructor + */ + CameraTrigger(); + + /** + * Destructor, also kills task. + */ + ~CameraTrigger(); + + /** + * Start the task. + */ + void start(); + + /** + * Stop the task. + */ + void stop(); + + /** + * Display info. + */ + void info(); + + int pin; + +private: + + struct hrt_call _pollcall; + struct hrt_call _firecall; + + int _gpio_fd; + + int _polarity; + float _activation_time; + float _integration_time; + float _transfer_time; + uint32_t _trigger_seq; + bool _trigger_enabled; + + hrt_abstime _trigger_timestamp; + + int _sensor_sub; + int _vcommand_sub; + + orb_advert_t _trigger_pub; + + struct camera_trigger_s _trigger; + struct sensor_combined_s _sensor; + struct vechicle_command_s _command; + + /** + * Topic poller to check for fire info. + */ + static void poll(void *arg); + /** + * Fires trigger + */ + static void engage(void *arg); + /** + * Resets trigger + */ + static void disengage(void *arg); + +}; + +namespace camera_trigger +{ + +CameraTrigger *g_camera_trigger; +} + +CameraTrigger::CameraTrigger() : + pin(1), + _gpio_fd(-1), + _polarity(0), + _activation_time(0.0f), + _integration_time(0.0f), + _transfer_time(0.0f), + _camera_trigger_sub(-1), + _trigger_seq(0), + _trigger_enabled(false), + _trigger{} +{ +} + +CameraTrigger::~CameraTrigger() +{ + camera_trigger::g_camera_trigger = nullptr; +} + +void +CameraTrigger::start() +{ + + /* Pull parameters */ + param_t polarity = param_find("TRIG_POLARITY"); + param_t activation_time = param_find("TRIG_ACT_TIME"); + param_t integration_time = param_find("TRIG_INT_TIME"); + param_t transfer_time = param_find("TRIG_TRANS_TIME"); + + param_get(polarity, &_polarity); + param_get(activation_time, &_activation_time); + param_get(integration_time, &_integration_time); + param_get(transfer_time, &_transfer_time); + + _gpio_fd = open(PX4FMU_DEVICE_PATH, 0); + + if (_gpio_fd < 0) { + + warnx("GPIO device open fail"); // TODO: errx + } + else + { + warnx("GPIO device opened"); + } + + _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); + + ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); + + if(_polarity == 0) + { + ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ + } + else if(_polarity == 1) + { + ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ + } + else + { + warnx(" invalid trigger polarity setting. stopping."); + stop(); + } + + hrt_call_every(&_pollcall, 0, 1000, (hrt_callout)&CameraTrigger::poll, this); +} + +void +CameraTrigger::stop() +{ + hrt_cancel(&_pollcall); + hrt_cancel(&_firecall); + + delete camera_trigger::g_camera_trigger; +} + +void +CameraTrigger::poll(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + bool updated; + orb_check(_vcommand_sub, &updated); + + if (updated) { + + orb_copy(ORB_ID(vehicle_command), _vcommand_sub, &_command); + + if(_command.command == VEHICLE_CMD_DO_TRIGGER_CONTROL) + { + if(_command.param1 < 1) + { + if(_trigger_enabled) + { + mavlink_log_info(_mavlink_fd, "camera trigger disabled"); + _trigger_enabled = false ; + } + } + else if(_command.param1 >= 1) + { + if(!_camera_trigger_enabled) + { + mavlink_log_info(_mavlink_fd, "camera trigger enabled"); + _trigger_enabled = true ; + } + } + + // Set trigger rate from command + if(_command.param2 > 0) + { + _trig->integration_time = _command.param2; + param_set(_trig->integration_time, &(_trig->_integration_time)); + } + } + } + + if(!_trigger_enabled) + return; + + if (hrt_elapsed_time(&_trigger_timestamp) > (_trig->_transfer_time + _trig->_integration_time)*1000 ) { + + engage(trig); + hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); + + _trigger_timestamp = hrt_absolute_time(); + + orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); + + _trigger.timestamp = _sensor.timestamp; + _trigger.seq = _trigger_seq++; + + if (_camera_trigger_pub > 0) { + orb_publish(ORB_ID(camera_trigger), _camera_trigger_pub, &_trigger); + } else { + _camera_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &_trigger); + } + + } + +} + +void +CameraTrigger::engage(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + if(trig->_polarity == 0) /* ACTIVE_LOW */ + { + ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + } + else if(trig->_polarity == 1) /* ACTIVE_HIGH */ + { + ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + } + +} + +void +CameraTrigger::disengage(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + if(trig->_polarity == 0) /* ACTIVE_LOW */ + { + ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + } + else if(trig->_polarity == 1) /* ACTIVE_HIGH */ + { + ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + } + +} + +void +CameraTrigger::info() +{ + warnx("Trigger state : %s", _trigger_enabled ? "enabled" : "disabled"); + warnx("Trigger pin : %i", pin); + warnx("Trigger polarity : %s", _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); + warnx("Shutter integration time : %.2f", (double)_integration_time); +} + +static void usage() +{ + errx(1, "usage: camera_trigger {start|stop|info} [-p ]\n" + "\t-p \tUse specified AUX OUT pin number (default: 1)" + ); +} + +int camera_trigger_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (camera_trigger::g_camera_trigger != nullptr) { + errx(0, "already running"); + } + + camera_trigger::g_camera_trigger = new CameraTrigger; + + if (camera_trigger::g_camera_trigger == nullptr) { + errx(1, "alloc failed"); + } + + if (argc > 3) { + + camera_trigger::g_camera_trigger->pin = (int)argv[3]; + if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) { + warnx("starting trigger on pin : %li ", atoi(argv[3])); + camera_trigger::g_camera_trigger->pin = atoi(argv[3]); + } + else + { + usage(); + } + } + camera_trigger::g_camera_trigger->start(); + + return 0; + } + + if (camera_trigger::g_camera_trigger == nullptr) { + errx(1, "not running"); + } + + else if (!strcmp(argv[1], "stop")) { + camera_trigger::g_camera_trigger->stop(); + + } + else if (!strcmp(argv[1], "info")) { + camera_trigger::g_camera_trigger->info(); + + } else { + usage(); + } + + return 0; +} + diff --git a/src/modules/camera_trigger/camera_trigger_params.c b/src/modules/camera_trigger/camera_trigger_params.c new file mode 100644 index 0000000000..f236e5aa2a --- /dev/null +++ b/src/modules/camera_trigger/camera_trigger_params.c @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file camera_trigger_params.c + * Camera trigger parameters + * + * @author Mohammed Kabir + */ + +#include +#include + +/** + * Camera trigger shutter integration time + * + * This parameter sets the time the shutter is open on the camera. + * + * @unit milliseconds + * @min 0.0 + * @max 500.0 + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_INT_TIME, 300.0f); + +/** + * Camera trigger transfer time + * + * This parameter sets the time the image transfer takes (PointGrey mode_0) + * + * @unit milliseconds + * @min 15.0 + * @max 33.0 + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_TRANS_TIME, 15.0f); + +/** + * Camera trigger polarity + * + * This parameter sets the polarity of the trigger (0 = ACTIVE_LOW, 1 = ACTIVE_HIGH ) + * + * @min 0 + * @max 1 + * @group Camera trigger + */ +PARAM_DEFINE_INT32(TRIG_POLARITY, 0); + +/** + * Camera trigger activation time + * + * This parameter sets the time the trigger needs to pulled high or low to start light + * integration. + * + * @unit milliseconds + * @default 4.0 ms + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 5.0f); diff --git a/src/modules/camera_trigger/module.mk b/src/modules/camera_trigger/module.mk new file mode 100644 index 0000000000..5bba057c5b --- /dev/null +++ b/src/modules/camera_trigger/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# External camera-IMU synchronisation via GPIO +# + +MODULE_COMMAND = camera_trigger +SRCS = camera_trigger.cpp \ + camera_trigger_params.c + +MODULE_STACKSIZE = 1000 +MAXOPTIMIZATION = -Os From 239c8dc7dc7e6ad9cc20cf7c02eec96350f87ee3 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 18 May 2015 09:14:39 +0530 Subject: [PATCH 020/493] camera trigger : implement trigerring and command --- makefiles/nuttx/config_px4fmu-v2_default.mk | 1 + src/modules/camera_trigger/camera_trigger.cpp | 88 +++++++++++-------- src/modules/uORB/objects_common.cpp | 3 + 3 files changed, 56 insertions(+), 36 deletions(-) diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk index 4c8f60e693..33fcf4d508 100644 --- a/makefiles/nuttx/config_px4fmu-v2_default.mk +++ b/makefiles/nuttx/config_px4fmu-v2_default.mk @@ -73,6 +73,7 @@ MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan MODULES += modules/land_detector +MODULES += modules/camera_trigger # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index f0c86e42f2..5a2970edea 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -53,9 +53,11 @@ #include #include #include +#include #include #include #include +#include extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); @@ -95,6 +97,7 @@ private: struct hrt_call _firecall; int _gpio_fd; + int _mavlink_fd; /**< mavlink log device handle */ int _polarity; float _activation_time; @@ -112,7 +115,12 @@ private: struct camera_trigger_s _trigger; struct sensor_combined_s _sensor; - struct vechicle_command_s _command; + struct vehicle_command_s _command; + + param_t polarity ; + param_t activation_time ; + param_t integration_time ; + param_t transfer_time ; /** * Topic poller to check for fire info. @@ -138,15 +146,28 @@ CameraTrigger *g_camera_trigger; CameraTrigger::CameraTrigger() : pin(1), _gpio_fd(-1), + _mavlink_fd(-1), _polarity(0), _activation_time(0.0f), _integration_time(0.0f), _transfer_time(0.0f), - _camera_trigger_sub(-1), _trigger_seq(0), - _trigger_enabled(false), + _trigger_enabled(true), + _trigger_timestamp(0), + _sensor_sub(-1), + _vcommand_sub(-1), + _trigger_pub(-1), _trigger{} { + memset(&_trigger, 0, sizeof(_trigger)); + memset(&_command, 0, sizeof(_command)); + memset(&_command, 0, sizeof(_sensor)); + + /* Parameters */ + polarity = param_find("TRIG_POLARITY"); + activation_time = param_find("TRIG_ACT_TIME"); + integration_time = param_find("TRIG_INT_TIME"); + transfer_time = param_find("TRIG_TRANS_TIME"); } CameraTrigger::~CameraTrigger() @@ -158,18 +179,8 @@ void CameraTrigger::start() { - /* Pull parameters */ - param_t polarity = param_find("TRIG_POLARITY"); - param_t activation_time = param_find("TRIG_ACT_TIME"); - param_t integration_time = param_find("TRIG_INT_TIME"); - param_t transfer_time = param_find("TRIG_TRANS_TIME"); - - param_get(polarity, &_polarity); - param_get(activation_time, &_activation_time); - param_get(integration_time, &_integration_time); - param_get(transfer_time, &_transfer_time); - _gpio_fd = open(PX4FMU_DEVICE_PATH, 0); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (_gpio_fd < 0) { @@ -182,6 +193,11 @@ CameraTrigger::start() _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); + + param_get(polarity, &_polarity); + param_get(activation_time, &_activation_time); + param_get(integration_time, &_integration_time); + param_get(transfer_time, &_transfer_time); ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); @@ -218,59 +234,59 @@ CameraTrigger::poll(void *arg) CameraTrigger *trig = reinterpret_cast(arg); bool updated; - orb_check(_vcommand_sub, &updated); + orb_check(trig->_vcommand_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_command), _vcommand_sub, &_command); + orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command); - if(_command.command == VEHICLE_CMD_DO_TRIGGER_CONTROL) + if(trig->_command.command == VEHICLE_CMD_DO_TRIGGER_CONTROL) { - if(_command.param1 < 1) + if(trig->_command.param1 < 1) { - if(_trigger_enabled) + if(trig->_trigger_enabled) { - mavlink_log_info(_mavlink_fd, "camera trigger disabled"); - _trigger_enabled = false ; + mavlink_log_info(trig->_mavlink_fd, "camera trigger disabled"); + trig->_trigger_enabled = false ; } } - else if(_command.param1 >= 1) + else if(trig->_command.param1 >= 1) { - if(!_camera_trigger_enabled) + if(!trig->_trigger_enabled) { - mavlink_log_info(_mavlink_fd, "camera trigger enabled"); - _trigger_enabled = true ; + mavlink_log_info(trig->_mavlink_fd, "camera trigger enabled"); + trig->_trigger_enabled = true ; } } // Set trigger rate from command - if(_command.param2 > 0) + if(trig->_command.param2 > 0) { - _trig->integration_time = _command.param2; - param_set(_trig->integration_time, &(_trig->_integration_time)); + trig->_integration_time = trig->_command.param2; + param_set(trig->integration_time, &(trig->_integration_time)); } } } - if(!_trigger_enabled) + if(!trig->_trigger_enabled) return; - if (hrt_elapsed_time(&_trigger_timestamp) > (_trig->_transfer_time + _trig->_integration_time)*1000 ) { + if (hrt_elapsed_time(&trig->_trigger_timestamp) > (trig->_transfer_time + trig->_integration_time)*1000 ) { engage(trig); hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); - _trigger_timestamp = hrt_absolute_time(); + trig->_trigger_timestamp = hrt_absolute_time(); orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); - _trigger.timestamp = _sensor.timestamp; - _trigger.seq = _trigger_seq++; + trig->_trigger.timestamp = trig->_sensor.timestamp; + trig->_trigger.seq = trig->_trigger_seq++; - if (_camera_trigger_pub > 0) { - orb_publish(ORB_ID(camera_trigger), _camera_trigger_pub, &_trigger); + if (trig->_trigger_pub > 0) { + orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger); } else { - _camera_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &_trigger); + trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 9f980eebea..a911168d92 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -256,3 +256,6 @@ ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); #include "topics/distance_sensor.h" ORB_DEFINE(distance_sensor, struct distance_sensor_s); + +#include "topics/camera_trigger.h" +ORB_DEFINE(camera_trigger, struct camera_trigger_s); From ecd2762281eede4d642b50bd028d46843f086b04 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 18 May 2015 10:03:00 +0530 Subject: [PATCH 021/493] camera trigger : fix memset --- src/modules/camera_trigger/camera_trigger.cpp | 28 +++++++++---------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 5a2970edea..564617bef7 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -97,7 +97,6 @@ private: struct hrt_call _firecall; int _gpio_fd; - int _mavlink_fd; /**< mavlink log device handle */ int _polarity; float _activation_time; @@ -146,7 +145,6 @@ CameraTrigger *g_camera_trigger; CameraTrigger::CameraTrigger() : pin(1), _gpio_fd(-1), - _mavlink_fd(-1), _polarity(0), _activation_time(0.0f), _integration_time(0.0f), @@ -161,7 +159,7 @@ CameraTrigger::CameraTrigger() : { memset(&_trigger, 0, sizeof(_trigger)); memset(&_command, 0, sizeof(_command)); - memset(&_command, 0, sizeof(_sensor)); + memset(&_sensor, 0, sizeof(_sensor)); /* Parameters */ polarity = param_find("TRIG_POLARITY"); @@ -180,11 +178,11 @@ CameraTrigger::start() { _gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (_gpio_fd < 0) { - warnx("GPIO device open fail"); // TODO: errx + warnx("GPIO device open fail"); // TODO + stop(); } else { @@ -214,8 +212,6 @@ CameraTrigger::start() warnx(" invalid trigger polarity setting. stopping."); stop(); } - - hrt_call_every(&_pollcall, 0, 1000, (hrt_callout)&CameraTrigger::poll, this); } void @@ -246,7 +242,6 @@ CameraTrigger::poll(void *arg) { if(trig->_trigger_enabled) { - mavlink_log_info(trig->_mavlink_fd, "camera trigger disabled"); trig->_trigger_enabled = false ; } } @@ -254,7 +249,6 @@ CameraTrigger::poll(void *arg) { if(!trig->_trigger_enabled) { - mavlink_log_info(trig->_mavlink_fd, "camera trigger enabled"); trig->_trigger_enabled = true ; } } @@ -262,14 +256,17 @@ CameraTrigger::poll(void *arg) // Set trigger rate from command if(trig->_command.param2 > 0) { - trig->_integration_time = trig->_command.param2; - param_set(trig->integration_time, &(trig->_integration_time)); + trig->_integration_time = trig->_command.param2; + param_set(trig->integration_time, &(trig->_integration_time)); } } } - if(!trig->_trigger_enabled) + if(!trig->_trigger_enabled) { + hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); return; + } + if (hrt_elapsed_time(&trig->_trigger_timestamp) > (trig->_transfer_time + trig->_integration_time)*1000 ) { @@ -280,7 +277,7 @@ CameraTrigger::poll(void *arg) orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); - trig->_trigger.timestamp = trig->_sensor.timestamp; + trig->_trigger.timestamp = trig->_sensor.timestamp; /* get IMU timestamp */ trig->_trigger.seq = trig->_trigger_seq++; if (trig->_trigger_pub > 0) { @@ -288,9 +285,10 @@ CameraTrigger::poll(void *arg) } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } - + + hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000 , (hrt_callout)&CameraTrigger::poll, trig); } - + } void From 34809e0aa37b3b76afcf23894f974b5db1635b66 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 18 May 2015 13:43:10 +0530 Subject: [PATCH 022/493] camera trigger : add message --- src/modules/uORB/topics/camera_trigger.h | 68 ++++++++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 src/modules/uORB/topics/camera_trigger.h diff --git a/src/modules/uORB/topics/camera_trigger.h b/src/modules/uORB/topics/camera_trigger.h new file mode 100644 index 0000000000..6994dffe5e --- /dev/null +++ b/src/modules/uORB/topics/camera_trigger.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file camera_trigger.h + * Camera-IMU synchronisation and triggering + */ + +#ifndef TOPIC_CAMERA_TRIGGER_H_ +#define TOPIC_CAMERA_TRIGGER_H_ + +#include +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Camera-IMU synchronisation message + */ +struct camera_trigger_s { + + uint64_t timestamp; /**< Timestamp when camera was triggered */ + uint32_t seq; /**< Image sequence - reset to zero on getting trigger reset command */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(camera_trigger); + +#endif /* TOPIC_CAMERA_TRIGGER_H_ */ + From be89a7262eae8ac95e421464a8fc98f2fd16d570 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 19 May 2015 22:46:04 +0530 Subject: [PATCH 023/493] camera trigger : add missing call to trampoline --- src/modules/camera_trigger/camera_trigger.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 564617bef7..89947c8fcf 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -212,6 +212,9 @@ CameraTrigger::start() warnx(" invalid trigger polarity setting. stopping."); stop(); } + + poll(this); + } void From 2dde99f0fc007520fadbf4ef4f45cd21d54528eb Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Wed, 20 May 2015 12:43:44 +0530 Subject: [PATCH 024/493] camera trigger : memset --- src/modules/camera_trigger/camera_trigger.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 89947c8fcf..b59443193a 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -161,6 +161,9 @@ CameraTrigger::CameraTrigger() : memset(&_command, 0, sizeof(_command)); memset(&_sensor, 0, sizeof(_sensor)); + memset(&_pollcall, 0, sizeof(_pollcall)); + memset(&_firecall, 0, sizeof(_firecall)); + /* Parameters */ polarity = param_find("TRIG_POLARITY"); activation_time = param_find("TRIG_ACT_TIME"); @@ -212,16 +215,16 @@ CameraTrigger::start() warnx(" invalid trigger polarity setting. stopping."); stop(); } - - poll(this); + + hrt_call_every(&_pollcall, 0, 1000, (hrt_callout)&CameraTrigger::poll, this); } void CameraTrigger::stop() -{ - hrt_cancel(&_pollcall); +{ hrt_cancel(&_firecall); + hrt_cancel(&_pollcall); delete camera_trigger::g_camera_trigger; } @@ -266,17 +269,15 @@ CameraTrigger::poll(void *arg) } if(!trig->_trigger_enabled) { - hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); return; } - if (hrt_elapsed_time(&trig->_trigger_timestamp) > (trig->_transfer_time + trig->_integration_time)*1000 ) { + if (hrt_elapsed_time(&trig->_trigger_timestamp) >= (trig->_transfer_time + trig->_integration_time)*1000 ) { engage(trig); - hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); - trig->_trigger_timestamp = hrt_absolute_time(); + hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); @@ -289,7 +290,6 @@ CameraTrigger::poll(void *arg) trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } - hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000 , (hrt_callout)&CameraTrigger::poll, trig); } } From 5ff38089e91034b3b6880b43c1fd391ff4ee1eac Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 21 May 2015 17:23:19 +0530 Subject: [PATCH 025/493] camera trigger : fix handling of fds in hrt callbacks --- src/modules/camera_trigger/camera_trigger.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index b59443193a..934945ada7 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -215,8 +215,9 @@ CameraTrigger::start() warnx(" invalid trigger polarity setting. stopping."); stop(); } - - hrt_call_every(&_pollcall, 0, 1000, (hrt_callout)&CameraTrigger::poll, this); + close(_gpio_fd); + + poll(this); /* Trampoline call */ } @@ -234,7 +235,7 @@ CameraTrigger::poll(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - + bool updated; orb_check(trig->_vcommand_sub, &updated); @@ -269,6 +270,7 @@ CameraTrigger::poll(void *arg) } if(!trig->_trigger_enabled) { + hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); return; } @@ -289,7 +291,7 @@ CameraTrigger::poll(void *arg) } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } - + hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000, (hrt_callout)&CameraTrigger::poll, trig); } } @@ -299,7 +301,9 @@ CameraTrigger::engage(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - + + trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); + if(trig->_polarity == 0) /* ACTIVE_LOW */ { ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); @@ -308,6 +312,8 @@ CameraTrigger::engage(void *arg) { ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); } + + close(trig->_gpio_fd); } @@ -316,7 +322,9 @@ CameraTrigger::disengage(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - + + trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); + if(trig->_polarity == 0) /* ACTIVE_LOW */ { ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); @@ -326,6 +334,8 @@ CameraTrigger::disengage(void *arg) ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); } + close(trig->_gpio_fd); + } void From 95a8e29cfeba7bf15c51e46db408bfc817f616c4 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 21 May 2015 17:41:52 +0530 Subject: [PATCH 026/493] camera trigger : mavlink stream --- msg/camera_trigger.msg | 4 ++ src/modules/camera_trigger/camera_trigger.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 54 +++++++++++++++++++ src/modules/uORB/topics/camera_trigger.h | 38 ++++++------- 5 files changed, 80 insertions(+), 19 deletions(-) create mode 100644 msg/camera_trigger.msg diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg new file mode 100644 index 0000000000..b4dcfe8ef3 --- /dev/null +++ b/msg/camera_trigger.msg @@ -0,0 +1,4 @@ + +uint64 timestamp # Timestamp when camera was triggered +uint32 seq # Image sequence + diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 934945ada7..0ee93128e4 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -184,7 +184,7 @@ CameraTrigger::start() if (_gpio_fd < 0) { - warnx("GPIO device open fail"); // TODO + warnx("GPIO device open fail"); stop(); } else diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ce2b5cb8bb..4e9e8a65f1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1611,6 +1611,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); + configure_stream("CAMERA_TRIGGER", 30.0f); break; case MAVLINK_MODE_OSD: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 24f04fd74f..2b34345a6b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -70,6 +70,7 @@ #include #include #include +#include #include #include #include @@ -1062,6 +1063,59 @@ protected: } }; + +class MavlinkStreamCameraTrigger : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamCameraTrigger::get_name_static(); + } + + static const char *get_name_static() { + return "CAMERA_TRIGGER"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_CAMERA_TRIGGER; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamCameraTrigger(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_camera_trigger_sub; + uint64_t _trig_time; + + /* do not allow top copying this class */ + MavlinkStreamCameraTrigger(MavlinkStreamCameraTrigger &); + MavlinkStreamCameraTrigger &operator = (const MavlinkStreamCameraTrigger &); + +protected: + explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink), + _camera_trigger_sub(_mavlink->add_orb_subscription(ORB_ID(camera_trigger))), + _trig_time(0) + {} + + void send(const hrt_abstime t) { + struct camera_trigger_s trigger; + + if (_camera_trigger_sub->update(&_trig_time, &trigger)) { + + mavlink_camera_trigger_t msg; + + msg.time_usec = trigger.timestamp; + msg.seq = trigger.seq; + + _mavlink->send_message(MAVLINK_MSG_ID_CAMERA_TRIGGER, &msg); + } + } +}; + class MavlinkStreamGlobalPositionInt : public MavlinkStream { public: diff --git a/src/modules/uORB/topics/camera_trigger.h b/src/modules/uORB/topics/camera_trigger.h index 6994dffe5e..a0d7e4cef8 100644 --- a/src/modules/uORB/topics/camera_trigger.h +++ b/src/modules/uORB/topics/camera_trigger.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,30 +31,35 @@ * ****************************************************************************/ -/** - * @file camera_trigger.h - * Camera-IMU synchronisation and triggering - */ +/* Auto-generated by genmsg_cpp from file /home/kabir/fork/Firmware/msg/camera_trigger.msg */ -#ifndef TOPIC_CAMERA_TRIGGER_H_ -#define TOPIC_CAMERA_TRIGGER_H_ -#include +#pragma once + #include +#include + + +#ifndef __cplusplus + +#endif /** * @addtogroup topics * @{ */ -/** - * Camera-IMU synchronisation message - */ -struct camera_trigger_s { - uint64_t timestamp; /**< Timestamp when camera was triggered */ - uint32_t seq; /**< Image sequence - reset to zero on getting trigger reset command */ - +#ifdef __cplusplus +struct __EXPORT camera_trigger_s { +#else +struct camera_trigger_s { +#endif + uint64_t timestamp; + uint32_t seq; +#ifdef __cplusplus + +#endif }; /** @@ -63,6 +68,3 @@ struct camera_trigger_s { /* register this as object request broker structure */ ORB_DECLARE(camera_trigger); - -#endif /* TOPIC_CAMERA_TRIGGER_H_ */ - From a1c2f24837301365d9731df644bf0138bc170342 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 21 May 2015 18:02:45 +0530 Subject: [PATCH 027/493] camera trigger : remove autogen message --- src/modules/uORB/topics/camera_trigger.h | 70 ------------------------ 1 file changed, 70 deletions(-) delete mode 100644 src/modules/uORB/topics/camera_trigger.h diff --git a/src/modules/uORB/topics/camera_trigger.h b/src/modules/uORB/topics/camera_trigger.h deleted file mode 100644 index a0d7e4cef8..0000000000 --- a/src/modules/uORB/topics/camera_trigger.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/kabir/fork/Firmware/msg/camera_trigger.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT camera_trigger_s { -#else -struct camera_trigger_s { -#endif - uint64_t timestamp; - uint32_t seq; -#ifdef __cplusplus - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(camera_trigger); From df037d97c152a16ca17ad0d84e5bacdd264093ba Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Fri, 22 May 2015 14:24:54 +0530 Subject: [PATCH 028/493] camera trigger : remove redundant timestamps --- src/modules/camera_trigger/camera_trigger.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 0ee93128e4..883048ba0c 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -105,8 +105,6 @@ private: uint32_t _trigger_seq; bool _trigger_enabled; - hrt_abstime _trigger_timestamp; - int _sensor_sub; int _vcommand_sub; @@ -151,7 +149,6 @@ CameraTrigger::CameraTrigger() : _transfer_time(0.0f), _trigger_seq(0), _trigger_enabled(true), - _trigger_timestamp(0), _sensor_sub(-1), _vcommand_sub(-1), _trigger_pub(-1), @@ -273,12 +270,9 @@ CameraTrigger::poll(void *arg) hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); return; } - - - if (hrt_elapsed_time(&trig->_trigger_timestamp) >= (trig->_transfer_time + trig->_integration_time)*1000 ) { - + else + { engage(trig); - trig->_trigger_timestamp = hrt_absolute_time(); hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); @@ -291,6 +285,7 @@ CameraTrigger::poll(void *arg) } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } + hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000, (hrt_callout)&CameraTrigger::poll, trig); } From 72e2224d1e87f32110c75b78554d4dc6130c797d Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Wed, 3 Jun 2015 11:48:37 +0530 Subject: [PATCH 029/493] camera trigger : master rebase --- msg/vehicle_command.msg | 3 ++- src/modules/camera_trigger/camera_trigger.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 391dc01aa0..2f223fbc2e 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -48,7 +48,8 @@ uint32 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutd uint32 VEHICLE_CMD_OVERRIDE_GOTO = 252 # Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| uint32 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| -uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 883048ba0c..c0a43f9255 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -151,7 +151,7 @@ CameraTrigger::CameraTrigger() : _trigger_enabled(true), _sensor_sub(-1), _vcommand_sub(-1), - _trigger_pub(-1), + _trigger_pub(nullptr), _trigger{} { memset(&_trigger, 0, sizeof(_trigger)); From af62e74d4a3f5a348cb98c95e688daed389db9dd Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Wed, 3 Jun 2015 11:51:02 +0530 Subject: [PATCH 030/493] camera trigger : command fix --- src/modules/camera_trigger/camera_trigger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index c0a43f9255..a694778014 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -240,7 +240,7 @@ CameraTrigger::poll(void *arg) orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command); - if(trig->_command.command == VEHICLE_CMD_DO_TRIGGER_CONTROL) + if(trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { if(trig->_command.param1 < 1) { From e6752e43b264a97bec830c442df518f7200fd854 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Fri, 5 Jun 2015 18:30:40 +0530 Subject: [PATCH 031/493] camera trigger : cleanup - still crashes --- src/modules/camera_trigger/camera_trigger.cpp | 55 +++++++++++-------- 1 file changed, 31 insertions(+), 24 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index a694778014..7c7e5a14d1 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -142,6 +142,8 @@ CameraTrigger *g_camera_trigger; CameraTrigger::CameraTrigger() : pin(1), + _pollcall{}, + _firecall{}, _gpio_fd(-1), _polarity(0), _activation_time(0.0f), @@ -152,11 +154,13 @@ CameraTrigger::CameraTrigger() : _sensor_sub(-1), _vcommand_sub(-1), _trigger_pub(nullptr), - _trigger{} + _trigger{}, + _sensor{}, + _command{} { memset(&_trigger, 0, sizeof(_trigger)); - memset(&_command, 0, sizeof(_command)); memset(&_sensor, 0, sizeof(_sensor)); + memset(&_command, 0, sizeof(_command)); memset(&_pollcall, 0, sizeof(_pollcall)); memset(&_firecall, 0, sizeof(_firecall)); @@ -180,7 +184,6 @@ CameraTrigger::start() _gpio_fd = open(PX4FMU_DEVICE_PATH, 0); if (_gpio_fd < 0) { - warnx("GPIO device open fail"); stop(); } @@ -197,15 +200,15 @@ CameraTrigger::start() param_get(integration_time, &_integration_time); param_get(transfer_time, &_transfer_time); - ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); + px4_ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); if(_polarity == 0) { - ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ + px4_ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ } else if(_polarity == 1) { - ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ + px4_ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ } else { @@ -224,7 +227,9 @@ CameraTrigger::stop() hrt_cancel(&_firecall); hrt_cancel(&_pollcall); - delete camera_trigger::g_camera_trigger; + if (camera_trigger::g_camera_trigger != nullptr) { + delete (camera_trigger::g_camera_trigger); + } } void @@ -280,7 +285,7 @@ CameraTrigger::poll(void *arg) trig->_trigger.timestamp = trig->_sensor.timestamp; /* get IMU timestamp */ trig->_trigger.seq = trig->_trigger_seq++; - if (trig->_trigger_pub > 0) { + if (trig->_trigger_pub != nullptr) { orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger); } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); @@ -298,17 +303,18 @@ CameraTrigger::engage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - - if(trig->_polarity == 0) /* ACTIVE_LOW */ + if(trig->_gpio_fd == -1) return; + + if(trig->_polarity == 0) // ACTIVE_LOW { - ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + px4_ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); } - else if(trig->_polarity == 1) /* ACTIVE_HIGH */ + else if(trig->_polarity == 1) // ACTIVE_HIGH { - ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + px4_ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); } - - close(trig->_gpio_fd); + + close(trig->_gpio_fd); } @@ -319,16 +325,17 @@ CameraTrigger::disengage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - - if(trig->_polarity == 0) /* ACTIVE_LOW */ - { - ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); - } - else if(trig->_polarity == 1) /* ACTIVE_HIGH */ - { - ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); - } + if(trig->_gpio_fd == -1) return; + if(trig->_polarity == 0) // ACTIVE_LOW + { + px4_ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + } + else if(trig->_polarity == 1) // ACTIVE_HIGH + { + px4_ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + } + close(trig->_gpio_fd); } From 9e3e43c49ee7f3e8b482b401f262a0bbd1868db4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 15:27:24 +0200 Subject: [PATCH 032/493] Update comments in attitude controller. Fixes #2369 --- src/modules/mc_att_control/mc_att_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 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 4136107414..1b5af55b19 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -599,10 +599,10 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll() } } -/* +/** * Attitude controller. - * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) - * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) + * Input: 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp' */ void MulticopterAttitudeControl::control_attitude(float dt) From 82352a64aaf341cddad5fc8e617a203ed6cf7285 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 19:36:29 +0200 Subject: [PATCH 033/493] commander: Remove unused param handles --- src/modules/commander/commander.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fa042ff47c..73550e41e9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -180,8 +180,6 @@ static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; -static float takeoff_alt = 5.0f; -static int parachute_enabled = 0; static float eph_threshold = 5.0f; static float epv_threshold = 10.0f; @@ -860,8 +858,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); - param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); @@ -1279,11 +1275,7 @@ int commander_thread_main(int argc, char *argv[]) rc_calibration_check(mavlink_fd); } - /* navigation parameters */ - param_get(_param_takeoff_alt, &takeoff_alt); - /* Safety parameters */ - param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); From 2ba8ac44382d7e88cc22b73471733c2d01d9305e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 17:31:31 +0200 Subject: [PATCH 034/493] Move mission result to generated topics --- msg/mission_result.msg | 11 ++++ src/modules/uORB/topics/mission_result.h | 75 ------------------------ 2 files changed, 11 insertions(+), 75 deletions(-) create mode 100644 msg/mission_result.msg delete mode 100644 src/modules/uORB/topics/mission_result.h diff --git a/msg/mission_result.msg b/msg/mission_result.msg new file mode 100644 index 0000000000..532db6f73d --- /dev/null +++ b/msg/mission_result.msg @@ -0,0 +1,11 @@ +uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified +uint32 seq_reached # Sequence of the mission item which has been reached +uint32 seq_current # Sequence of the current mission item +bool valid # true if mission is valid +bool reached # true if mission has been reached +bool finished # true if mission has been completed +bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode +bool flight_termination # true if the navigator demands a flight termination from the commander app +bool item_do_jump_changed # true if the number of do jumps remaining has changed +uint32 item_changed_index # indicate which item has changed +uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h deleted file mode 100644 index 16e7f2f126..0000000000 --- a/src/modules/uORB/topics/mission_result.h +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mission_result.h - * Mission results that navigator needs to pass on to commander and mavlink. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * @author Ban Siesta - */ - -#ifndef TOPIC_MISSION_RESULT_H -#define TOPIC_MISSION_RESULT_H - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct mission_result_s { - unsigned seq_reached; /**< Sequence of the mission item which has been reached */ - unsigned seq_current; /**< Sequence of the current mission item */ - bool reached; /**< true if mission has been reached */ - bool finished; /**< true if mission has been completed */ - bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ - bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ - bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */ - unsigned item_changed_index; /**< indicate which item has changed */ - unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(mission_result); - -#endif From 2cf10a5e999ce1e5c2579a202755346a6ad18046 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 17:31:58 +0200 Subject: [PATCH 035/493] Navigator: Publish mission validity in mission result --- src/modules/navigator/mission.cpp | 9 +++++++++ src/modules/navigator/navigator.h | 3 +++ src/modules/navigator/navigator_main.cpp | 4 ++++ 3 files changed, 16 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a74e042a91..3848d16e5d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -197,6 +197,11 @@ Mission::update_onboard_mission() /* otherwise, just leave it */ } + // XXX check validity here as well + _navigator->get_mission_result()->valid = true; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + } else { _onboard_mission.count = 0; _onboard_mission.current_seq = 0; @@ -234,6 +239,10 @@ 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_mission_result()->valid = !failed; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + } else { warnx("offboard mission update failed"); } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 782a297fbb..093e1be3c2 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -163,6 +163,8 @@ public: float get_acceptance_radius(float mission_item_radius); int get_mavlink_fd() { return _mavlink_fd; } + void increment_mission_instance_count() { _mission_instance_count++; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -205,6 +207,7 @@ private: bool _home_position_set; bool _mission_item_valid; /**< flags if the current mission item is valid */ + int _mission_instance_count; /**< instance count for the current mission */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8cfce50879..1460972cc2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -125,6 +125,7 @@ Navigator::Navigator() : _att_sp{}, _home_position_set(false), _mission_item_valid(false), + _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), @@ -663,6 +664,8 @@ int navigator_main(int argc, char *argv[]) void Navigator::publish_mission_result() { + _mission_result.instance_count = _mission_instance_count; + /* lazily publish the mission result only once available */ if (_mission_result_pub > 0) { /* publish mission result */ @@ -679,6 +682,7 @@ Navigator::publish_mission_result() _mission_result.item_do_jump_changed = false; _mission_result.item_changed_index = 0; _mission_result.item_do_jump_remaining = 0; + _mission_result.valid = true; } void From a4b238946070dfa2094b87d8fff61a987a6bec03 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 18:37:19 +0200 Subject: [PATCH 036/493] Commander: Support new mission status --- src/modules/commander/commander.cpp | 26 ++++++++++++++++- src/modules/commander/commander_helper.cpp | 33 ++++++++++++++++++++++ src/modules/commander/commander_helper.h | 3 ++ 3 files changed, 61 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 73550e41e9..edeb31e888 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -190,6 +190,8 @@ static struct vehicle_control_mode_s control_mode; static struct offboard_control_mode_s offboard_control_mode; static struct home_position_s _home; +static unsigned _last_mission_instance = 0; + /** * The daemon app only briefly exists to start * the background job. The stack size assigned in the @@ -839,7 +841,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & //Play tune first time we initialize HOME if (!status.condition_home_position_valid) { - tune_positive(true); + tune_home_set(true); } /* mark home position as set */ @@ -1764,6 +1766,28 @@ int commander_thread_main(int argc, char *argv[]) } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + /* Only evaluate mission state if home is set, + * this prevents false positives for the mission + * rejection. Back off 2 seconds to not overlay + * home tune. + */ + if (status.condition_home_position_valid && + (hrt_elapsed_time(&_home.timestamp) > 2000000) && + _last_mission_instance != mission_result.instance_count) { + if (mission_result.valid) { + /* the mission is valid */ + tune_mission_ok(true); + warnx("mission ok"); + } else { + /* the mission is not valid */ + tune_mission_fail(true); + warnx("mission fail"); + } + + /* prevent further feedback until the mission changes */ + _last_mission_instance = mission_result.instance_count; + } + /* RC input check */ if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index c0f8561fda..cbf11de1b0 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -172,6 +172,39 @@ void set_tune(int tune) } } +void tune_home_set(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } +} + +void tune_mission_ok(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } +} + +void tune_mission_fail(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } +} + /** * Blink green LED and play positive tune (if use_buzzer == true). */ diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index d2aace2a40..d2ab41f887 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -58,6 +58,9 @@ void buzzer_deinit(void); void set_tune_override(int tune); void set_tune(int tune); +void tune_home_set(bool use_buzzer); +void tune_mission_ok(bool use_buzzer); +void tune_mission_fail(bool use_buzzer); void tune_positive(bool use_buzzer); void tune_neutral(bool use_buzzer); void tune_negative(bool use_buzzer); From 174f4d27f3e65454808e073023ee14833f3d7ff2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 18:37:32 +0200 Subject: [PATCH 037/493] Navigator: output new mission status --- src/modules/navigator/mission.cpp | 18 ++++++++++++++++++ src/modules/navigator/mission.h | 1 + 2 files changed, 19 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3848d16e5d..be27e6208a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -78,6 +78,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _takeoff(false), _mission_type(MISSION_TYPE_NONE), _inited(false), + _home_inited(false), _dist_1wp_ok(false), _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), @@ -110,6 +111,22 @@ Mission::on_inactive() update_offboard_mission(); } + /* check if the home position became valid in the meantime */ + if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) && + !_home_inited && _navigator->home_position_valid()) { + + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + + _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt, _navigator->home_position_valid()); + + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + + _home_inited = true; + } + } else { /* read mission topics on initialization */ _inited = true; @@ -176,6 +193,7 @@ Mission::on_active() && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); } + } void diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index bc9a2c6c82..6cfae49598 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -186,6 +186,7 @@ private: } _mission_type; bool _inited; + bool _home_inited; bool _dist_1wp_ok; MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ From 21ca431131e5c50854a75d55655d4c87b268cea8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 15:12:17 +0200 Subject: [PATCH 038/493] Tone alarm: Add home set tune --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 31aee266e1..3506d832d0 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -152,6 +152,7 @@ enum { TONE_EKF_WARNING_TUNE, TONE_BARO_WARNING_TUNE, TONE_SINGLE_BEEP_TUNE, + TONE_HOME_SET, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a18b54981f..bf8418bf9f 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -339,6 +339,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning _default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning _default_tunes[TONE_SINGLE_BEEP_TUNE] = "MFT100a8"; // single beep + _default_tunes[TONE_HOME_SET] = "MFT100L4>G#6A#6B#4"; _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -354,6 +355,7 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning _tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning _tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep + _tune_names[TONE_HOME_SET] = "home_set"; } ToneAlarm::~ToneAlarm() From b5a79bbc0b22a83c7a4b3cefaf7f1195f5b05f32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 15:12:35 +0200 Subject: [PATCH 039/493] commander: Use distinct tunes for home set and mission ok / failed --- src/modules/commander/commander_helper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index cbf11de1b0..362a707c03 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -179,7 +179,7 @@ void tune_home_set(bool use_buzzer) rgbled_set_mode(RGBLED_MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(TONE_HOME_SET); } } @@ -190,7 +190,7 @@ void tune_mission_ok(bool use_buzzer) rgbled_set_mode(RGBLED_MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(TONE_NOTIFY_NEUTRAL_TUNE); } } @@ -201,7 +201,7 @@ void tune_mission_fail(bool use_buzzer) rgbled_set_mode(RGBLED_MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); } } From eb3cc8b41ab587f7cd6240abb9dcba918888218c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:02:55 +0200 Subject: [PATCH 040/493] mission result topic: Add warnings --- msg/mission_result.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/mission_result.msg b/msg/mission_result.msg index 532db6f73d..ac4d32f559 100644 --- a/msg/mission_result.msg +++ b/msg/mission_result.msg @@ -2,6 +2,7 @@ uint32 instance_count # Instance count of this mission. Increments monotonically uint32 seq_reached # Sequence of the mission item which has been reached uint32 seq_current # Sequence of the current mission item bool valid # true if mission is valid +bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings bool reached # true if mission has been reached bool finished # true if mission has been completed bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode From b11e13331882272c6b6bd5d200ec938070ade632 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:03:12 +0200 Subject: [PATCH 041/493] Evaluate warning field from mission result --- src/modules/commander/commander.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index edeb31e888..5ac8e9ca8b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1774,14 +1774,18 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_home_position_valid && (hrt_elapsed_time(&_home.timestamp) > 2000000) && _last_mission_instance != mission_result.instance_count) { - if (mission_result.valid) { + if (!mission_result.valid) { + /* the mission is invalid */ + tune_mission_fail(true); + warnx("mission fail"); + } else if (mission_result.warning) { + /* the mission has a warning */ + tune_mission_fail(true); + warnx("mission warning"); + } else { /* the mission is valid */ tune_mission_ok(true); warnx("mission ok"); - } else { - /* the mission is not valid */ - tune_mission_fail(true); - warnx("mission fail"); } /* prevent further feedback until the mission changes */ From 41f535ae262d7a6e5f4a2fe043b66d86dde9993e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:03:34 +0200 Subject: [PATCH 042/493] navigator: Include distance to first waypoint in mission check, provide warning feedback --- src/modules/navigator/mission.cpp | 82 ++----------- src/modules/navigator/mission.h | 1 - .../navigator/mission_feasibility_checker.cpp | 113 ++++++++++++++---- .../navigator/mission_feasibility_checker.h | 9 +- 4 files changed, 107 insertions(+), 98 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index be27e6208a..4944ebe789 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -79,7 +79,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_type(MISSION_TYPE_NONE), _inited(false), _home_inited(false), - _dist_1wp_ok(false), _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), @@ -119,7 +118,9 @@ Mission::on_inactive() _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _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->increment_mission_instance_count(); _navigator->set_mission_result_updated(); @@ -255,7 +256,9 @@ Mission::update_offboard_mission() failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _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_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -308,73 +311,6 @@ Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item) } } -bool -Mission::check_dist_1wp() -{ - if (_dist_1wp_ok) { - /* always return true after at least one successful check */ - return true; - } - - /* check if first waypoint is not too far from home */ - if (_param_dist_1wp.get() > 0.0f) { - if (_navigator->get_vstatus()->condition_home_position_valid) { - struct mission_item_s mission_item; - - /* find first waypoint (with lat/lon) item in datamanager */ - for (unsigned i = 0; i < _offboard_mission.count; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, - &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { - - /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || - mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || - mission_item.nav_cmd == NAV_CMD_TAKEOFF || - mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { - - /* check distance from current position to item */ - float dist_to_1wp = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - - if (dist_to_1wp < _param_dist_1wp.get()) { - _dist_1wp_ok = true; - if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { - /* allow at 2/3 distance, but warn */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); - } - return true; - - } else { - /* item is too far from home */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get()); - return false; - } - } - - } else { - /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission"); - return false; - } - } - - /* no waypoints found in mission, then we will not fly far away */ - _dist_1wp_ok = true; - return true; - - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "no home position"); - return false; - } - - } else { - return true; - } -} - void Mission::set_mission_items() { @@ -394,10 +330,8 @@ Mission::set_mission_items() _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item); } - /* get home distance state */ - bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ - bool user_feedback_done = !home_dist_ok; + bool user_feedback_done = false; /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { @@ -409,7 +343,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { + } else if (read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6cfae49598..d77f461574 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -187,7 +187,6 @@ private: bool _inited; bool _home_inited; - bool _dist_1wp_ok; MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 9d1dc7c7e6..05019bf8aa 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -57,23 +57,40 @@ #endif static const int ERROR = -1; -MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false) +MissionFeasibilityChecker::MissionFeasibilityChecker() : + _mavlink_fd(-1), + _capabilities_sub(-1), + _initDone(false), + _dist_1wp_ok(false) { _nav_caps = {0}; } -bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +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) { bool failed = false; + bool warned = false; /* Init if not done yet */ init(); _mavlink_fd = mavlink_fd; + // first check if we have a valid position + if (!home_valid /* can later use global / local pos for finer granularity */) { + failed = true; + warned = true; + mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock."); + } else { + failed |= !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); + } + // check if all mission item commands are supported failed |= !checkMissionItemValidity(dm_current, nMissionItems); - + failed |= !checkGeofence(dm_current, nMissionItems, geofence); + failed |= !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); @@ -90,28 +107,20 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { - - /* Perform checks and issue feedback to the user for all checks */ - bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); - - /* Mission is only marked as feasible if all checks return true */ - return (resGeofence && resHomeAltitude); + /* no custom rotary wing checks yet */ + return true; } bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); -// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); /* Perform checks and issue feedback to the user for all checks */ bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); - bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ - return (resLanding && resGeofence && resHomeAltitude); + return resLanding; } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -137,7 +146,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } -bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error) +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, + float home_alt, bool home_valid, bool &warning_issued, bool throw_error) { /* 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++) { @@ -145,17 +155,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { + warning_issued = true; /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - if (throw_error) { - return false; - } else { - return true; - } + return false; } /* always reject relative alt without home set */ if (missionitem.altitude_is_relative && !home_valid) { mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + warning_issued = true; return false; } @@ -163,6 +171,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; if (home_alt > wp_alt) { + + warning_issued = true; + if (throw_error) { mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; @@ -275,6 +286,68 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size return true; } +bool +MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued) +{ + if (_dist_1wp_ok) { + /* always return true after at least one successful check */ + return true; + } + + /* check if first waypoint is not too far from home */ + if (dist_first_wp > 0.0f) { + struct mission_item_s mission_item; + + /* find first waypoint (with lat/lon) item in datamanager */ + for (unsigned i = 0; i < nMissionItems; i++) { + if (dm_read(dm_current, i, + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + + /* check only items with valid lat/lon */ + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + mission_item.nav_cmd == NAV_CMD_TAKEOFF || + mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + + /* check distance from current position to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, curr_lat, curr_lon); + + if (dist_to_1wp < dist_first_wp) { + _dist_1wp_ok = true; + if (dist_to_1wp > ((dist_first_wp * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + warning_issued = true; + } + return true; + + } else { + /* item is too far from home */ + mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp); + warning_issued = true; + return false; + } + } + + } else { + /* error reading, mission is invalid */ + mavlink_log_info(_mavlink_fd, "error reading offboard mission"); + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + _dist_1wp_ok = true; + return true; + + } else { + return true; + } +} + void MissionFeasibilityChecker::updateNavigationCapabilities() { (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 9c9511be3d..4586f75a47 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -57,12 +57,14 @@ private: struct navigation_capabilities_s _nav_caps; bool _initDone; + bool _dist_1wp_ok; void init(); /* 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 throw_error = false); + 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 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 */ bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); @@ -79,8 +81,9 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt, bool home_valid); + 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); }; From 73d179fb59eb996d7cf95ab524b399c8efc8d407 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Apr 2015 01:29:51 +0200 Subject: [PATCH 043/493] MS5611 driver: Fix reeset logic via I2C, minor code style fixes. Fixes #2007, identified by Kirill-ka --- src/drivers/ms5611/ms5611.cpp | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index ef94d03633..18b02228c2 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -154,10 +154,12 @@ protected: /** * Initialize the automatic measurement state machine and start it. * + * @param delay_ticks the number of queue ticks before executing the next cycle + * * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start_cycle(); + void start_cycle(unsigned delay_ticks = 1); /** * Stop the automatic measurement state machine. @@ -515,7 +517,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start_cycle() +MS5611::start_cycle(unsigned delay_ticks) { /* reset the report ring and state machine */ @@ -524,7 +526,7 @@ MS5611::start_cycle() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, delay_ticks); } void @@ -564,8 +566,11 @@ MS5611::cycle() } /* issue a reset command to the sensor */ _interface->ioctl(IOCTL_RESET, dummy); - /* reset the collection state machine and try again */ - start_cycle(); + /* reset the collection state machine and try again - we need + * to wait 2.8 ms after issuing the sensor reset command + * according to the MS5611 datasheet + */ + start_cycle(USEC2TICK(2800)); return; } @@ -594,7 +599,6 @@ MS5611::cycle() /* measurement phase */ ret = measure(); if (ret != OK) { - //log("measure error %d", ret); /* issue a reset command to the sensor */ _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ @@ -1182,26 +1186,30 @@ ms5611_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ms5611::start(busid); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { ms5611::test(busid); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { ms5611::reset(busid); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { ms5611::info(); + } /* * Perform MSL pressure calibration given an altitude in metres From 0dc6e65d7a9814196331736cfa4f019a8e56dcef Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 15 Jun 2015 23:24:14 +0530 Subject: [PATCH 044/493] camera_trigger : direct GPIO access. finally working. --- src/modules/camera_trigger/camera_trigger.cpp | 39 ++++++------------- src/modules/camera_trigger/module.mk | 1 - 2 files changed, 11 insertions(+), 29 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 7c7e5a14d1..2a1632d1e4 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -59,6 +59,8 @@ #include #include +#include + extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); class CameraTrigger @@ -181,17 +183,6 @@ void CameraTrigger::start() { - _gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - - if (_gpio_fd < 0) { - warnx("GPIO device open fail"); - stop(); - } - else - { - warnx("GPIO device opened"); - } - _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -199,24 +190,21 @@ CameraTrigger::start() param_get(activation_time, &_activation_time); param_get(integration_time, &_integration_time); param_get(transfer_time, &_transfer_time); - - px4_ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); if(_polarity == 0) { - px4_ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ + //px4_ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ } else if(_polarity == 1) { - px4_ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ + //px4_ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ } else { warnx(" invalid trigger polarity setting. stopping."); stop(); } - close(_gpio_fd); - + poll(this); /* Trampoline call */ } @@ -302,19 +290,17 @@ CameraTrigger::engage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); - trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - if(trig->_gpio_fd == -1) return; + stm32_configgpio(GPIO_GPIO0_OUTPUT); if(trig->_polarity == 0) // ACTIVE_LOW { - px4_ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); } else if(trig->_polarity == 1) // ACTIVE_HIGH { - px4_ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); } - close(trig->_gpio_fd); } @@ -324,19 +310,16 @@ CameraTrigger::disengage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); - trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - if(trig->_gpio_fd == -1) return; + stm32_configgpio(GPIO_GPIO0_OUTPUT); if(trig->_polarity == 0) // ACTIVE_LOW { - px4_ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); } else if(trig->_polarity == 1) // ACTIVE_HIGH { - px4_ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); } - - close(trig->_gpio_fd); } diff --git a/src/modules/camera_trigger/module.mk b/src/modules/camera_trigger/module.mk index 5bba057c5b..54098cc855 100644 --- a/src/modules/camera_trigger/module.mk +++ b/src/modules/camera_trigger/module.mk @@ -39,5 +39,4 @@ MODULE_COMMAND = camera_trigger SRCS = camera_trigger.cpp \ camera_trigger_params.c -MODULE_STACKSIZE = 1000 MAXOPTIMIZATION = -Os From 677aef6673e96f7227db981a2e464898c86290ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 21:55:02 +0200 Subject: [PATCH 045/493] navigator: Fixed bitwise or --- .../navigator/mission_feasibility_checker.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 05019bf8aa..c57a12aefb 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -84,18 +84,18 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota warned = true; mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock."); } else { - failed |= !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); + failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); } // check if all mission item commands are supported - failed |= !checkMissionItemValidity(dm_current, nMissionItems); - failed |= !checkGeofence(dm_current, nMissionItems, geofence); - failed |= !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); + failed = failed || !checkMissionItemValidity(dm_current, nMissionItems); + failed = failed || !checkGeofence(dm_current, nMissionItems, geofence); + failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { - failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); } else { - failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } if (!failed) { From 460c6bcf5715aac96bbb92ab225676b6b62a8c0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 21:56:44 +0200 Subject: [PATCH 046/493] MC att control demand: Require a higher minimum throttle --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 ++++--- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 995937aa6c..6eaca26b17 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1396,14 +1396,15 @@ MulticopterPositionControl::task_main() } //Control roll and pitch directly if we no aiding velocity controller is active - if(!_control_mode.flag_control_velocity_enabled) { + if (!_control_mode.flag_control_velocity_enabled) { _att_sp.roll_body = _manual.y * _params.man_roll_max; _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; } //Control climb rate directly if no aiding altitude controller is active - if(!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER); + if (!_control_mode.flag_control_climb_rate_enabled) { + _att_sp.thrust = math::min(_manual.z, _params.thr_max); + _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); } //Construct attitude setpoint rotation matrix 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 ade43ffb91..4a58dfdd0c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -50,7 +50,7 @@ * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.18f); /** * Maximum thrust From 1ebea1e7590d0d66b18cd39fcc45897bca8b8256 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 11:05:44 +0200 Subject: [PATCH 047/493] ask for climbout mode when doin takeoff help --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 01d94a8881..8e516a708f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -395,7 +395,7 @@ private: /** * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + bool do_takeoff_help(); /** * Update desired altitude base on user pitch stick input @@ -987,16 +987,20 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -void FixedwingPositionControl::do_takeoff_help() +bool FixedwingPositionControl::do_takeoff_help() { const hrt_abstime delta_takeoff = 10000000; - const float throttle_threshold = 0.3f; - const float delta_alt_takeoff = 30.0f; + const float throttle_threshold = 0.1f; + const float delta_alt_takeoff = 50.0f; + float climbout = false; /* demand 30 m above ground if user switched into this mode during takeoff */ if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { _hold_alt = _ground_alt + delta_alt_takeoff; + climbout = true; + } + return climbout; } bool @@ -1416,7 +1420,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1509,7 +1513,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; From 79944b2c354e15ebbb47901d01d6542060b84c05 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 16 Jun 2015 11:21:36 +0200 Subject: [PATCH 048/493] Update pitch and yaw gains to flight tested values. --- ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index bd99e0e554..36d387c759 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -9,16 +9,15 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then - # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.13 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_P 0.19 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.8 + param set MC_YAW_P 4.0 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 From ba89883fb012d4685b948a9afcb07a4fa5536e76 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 16 Jun 2015 15:44:58 +0530 Subject: [PATCH 049/493] camera trigger: minor cleanup --- src/modules/camera_trigger/camera_trigger.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 2a1632d1e4..a81607e950 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -58,7 +58,6 @@ #include #include #include - #include extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); @@ -152,7 +151,7 @@ CameraTrigger::CameraTrigger() : _integration_time(0.0f), _transfer_time(0.0f), _trigger_seq(0), - _trigger_enabled(true), + _trigger_enabled(false), _sensor_sub(-1), _vcommand_sub(-1), _trigger_pub(nullptr), @@ -191,16 +190,15 @@ CameraTrigger::start() param_get(integration_time, &_integration_time); param_get(transfer_time, &_transfer_time); - if(_polarity == 0) - { - //px4_ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ + stm32_configgpio(GPIO_GPIO0_OUTPUT); + + if(_polarity == 0) { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); /* GPIO pin pull high */ } - else if(_polarity == 1) - { - //px4_ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ + else if(_polarity == 1) { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); /* GPIO pin pull low */ } - else - { + else { warnx(" invalid trigger polarity setting. stopping."); stop(); } From 6a818ae05315824085ed89598d33a3e3b9e147bc Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 16 Jun 2015 15:47:55 +0530 Subject: [PATCH 050/493] commander : ignore handling camera_trigger command --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d47b45d89f..19e9b93ab9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -783,6 +783,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: + case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL: /* ignore commands that handled in low prio loop */ break; From 3d92364d9eb391d3f0d615df7092d96194e2d5b0 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 16 Jun 2015 22:55:05 +0530 Subject: [PATCH 051/493] camera trigger : increase free cycling time when we are not enabled --- src/modules/camera_trigger/camera_trigger.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index a81607e950..bfbd770c8b 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -166,7 +166,7 @@ CameraTrigger::CameraTrigger() : memset(&_pollcall, 0, sizeof(_pollcall)); memset(&_firecall, 0, sizeof(_firecall)); - /* Parameters */ + // Parameters polarity = param_find("TRIG_POLARITY"); activation_time = param_find("TRIG_ACT_TIME"); integration_time = param_find("TRIG_INT_TIME"); @@ -193,17 +193,17 @@ CameraTrigger::start() stm32_configgpio(GPIO_GPIO0_OUTPUT); if(_polarity == 0) { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); /* GPIO pin pull high */ + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); // GPIO pin pull high } else if(_polarity == 1) { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); /* GPIO pin pull low */ + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); // GPIO pin pull low } else { warnx(" invalid trigger polarity setting. stopping."); stop(); } - poll(this); /* Trampoline call */ + poll(this); // Trampoline call } @@ -258,7 +258,7 @@ CameraTrigger::poll(void *arg) } if(!trig->_trigger_enabled) { - hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); + hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig); return; } else @@ -268,7 +268,7 @@ CameraTrigger::poll(void *arg) orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); - trig->_trigger.timestamp = trig->_sensor.timestamp; /* get IMU timestamp */ + trig->_trigger.timestamp = trig->_sensor.timestamp; // get IMU timestamp trig->_trigger.seq = trig->_trigger_seq++; if (trig->_trigger_pub != nullptr) { From c91bb76b42d8d85339902c0b29123243156a0f0b Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 11:05:44 +0200 Subject: [PATCH 052/493] ask for climbout mode when doin takeoff help --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 01d94a8881..8e516a708f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -395,7 +395,7 @@ private: /** * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + bool do_takeoff_help(); /** * Update desired altitude base on user pitch stick input @@ -987,16 +987,20 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -void FixedwingPositionControl::do_takeoff_help() +bool FixedwingPositionControl::do_takeoff_help() { const hrt_abstime delta_takeoff = 10000000; - const float throttle_threshold = 0.3f; - const float delta_alt_takeoff = 30.0f; + const float throttle_threshold = 0.1f; + const float delta_alt_takeoff = 50.0f; + float climbout = false; /* demand 30 m above ground if user switched into this mode during takeoff */ if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { _hold_alt = _ground_alt + delta_alt_takeoff; + climbout = true; + } + return climbout; } bool @@ -1416,7 +1420,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1509,7 +1513,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - do_takeoff_help(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; From 5c59d7a434791222a6fad49e64206432a86c22bf Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 23:05:25 +0200 Subject: [PATCH 053/493] do not run tecs if we are on ground to prevent integrator filling --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 8e516a708f..05988ef53a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1024,7 +1024,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled()) { + if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); } @@ -1757,6 +1757,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, unsigned mode, bool pitch_max_special) { + /* do not run tecs if we are not in air */ + if (_vehicle_status.condition_landed) { + return; + } + if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; From 6ce106eea465b9bbaf59f4d44a2954bd107d1035 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 17 Jun 2015 17:36:26 +0200 Subject: [PATCH 054/493] limit minimum pitch in altitude controller modes if in a takeoff situation --- .../fw_pos_control_l1_main.cpp | 63 +++++++++++++------ 1 file changed, 45 insertions(+), 18 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 05988ef53a..c262e80c5f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -389,13 +389,16 @@ private: float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** - * Control position. + * Check if we are in a takeoff situation */ + bool in_takeoff_situation(); /** * Do takeoff help when in altitude controlled modes + * @param hold_altitude altitude setpoint for controller + * @param pitch_limit_min minimum pitch allowed */ - bool do_takeoff_help(); + void do_takeoff_help(float *hold_altitude, float *pitch_limit_min); /** * Update desired altitude base on user pitch stick input @@ -405,6 +408,9 @@ private: */ bool update_desired_altitude(float dt); + /** + * Control position. + */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); @@ -987,20 +993,26 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -bool FixedwingPositionControl::do_takeoff_help() -{ +bool FixedwingPositionControl::in_takeoff_situation() { const hrt_abstime delta_takeoff = 10000000; const float throttle_threshold = 0.1f; - const float delta_alt_takeoff = 50.0f; - float climbout = false; - - /* demand 30 m above ground if user switched into this mode during takeoff */ - if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { - _hold_alt = _ground_alt + delta_alt_takeoff; - climbout = true; + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) { + return true; + } + + return false; +} + +void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min) +{ + /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ + if (in_takeoff_situation()) { + *hold_altitude = _ground_alt + _parameters.climbout_diff; + *pitch_limit_min = math::radians(10.0f); + } else { + *pitch_limit_min = _parameters.pitch_limit_min; } - return climbout; } bool @@ -1419,8 +1431,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - climbout_requested |= do_takeoff_help(); + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); + /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1437,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - math::radians(_parameters.pitch_limit_min), + pitch_limit_min, _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); @@ -1452,6 +1468,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } + /* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration + to make sure the plane does not start rolling + */ + if (in_takeoff_situation()) { + _hdg_hold_enabled = false; + _yaw_lock_engaged = true; + } + if (_yaw_lock_engaged) { /* just switched back from non heading-hold to heading hold */ @@ -1512,8 +1536,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - climbout_requested |= do_takeoff_help(); + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1530,7 +1557,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - math::radians(_parameters.pitch_limit_min), + pitch_limit_min, _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); From 0446efa9a4b8e9399f64d4c65303bab971342b3e Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 17 Jun 2015 17:46:37 +0200 Subject: [PATCH 055/493] limit roll angle in loiter and position control mode if we are in a takeoff situation --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c262e80c5f..e4682689af 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1134,6 +1134,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + if (in_takeoff_situation()) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + } + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1505,6 +1511,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + + if (in_takeoff_situation()) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + } } } else { _hdg_hold_enabled = false; From 3e64ad10e8e53219c9d30f17311dc7f6db57b173 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 17 Jun 2015 06:21:28 -1000 Subject: [PATCH 056/493] Conditional inclusion of the Node Allocation and FW Server - default is OFF --- src/modules/uavcan/uavcan_main.cpp | 25 ++++++++++++++++++------- src/modules/uavcan/uavcan_main.hpp | 21 ++++++++++++--------- 2 files changed, 30 insertions(+), 16 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c18d6e5d1c..f4763fce7f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -53,14 +53,15 @@ #include #include "uavcan_main.hpp" -#include -#include - -#include +#if defined(USE_FW_NODE_SERVER) +# include +# include +# include //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined -#define OK 0 +# define OK 0 +#endif /** * @file uavcan_main.cpp @@ -75,20 +76,26 @@ * UavcanNode */ UavcanNode *UavcanNode::_instance; +#if defined(USE_FW_NODE_SERVER) uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance; uavcan_posix::dynamic_node_id_server::FileEventTracer tracer; uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; uavcan_posix::FirmwareVersionChecker fw_version_checker; - +#endif UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), +#if !defined(USE_FW_NODE_SERVER) + _esc_controller(_node) +#else _esc_controller(_node), _fileserver_backend(_node), _node_info_retriever(_node), _fw_upgrade_trigger(_node, fw_version_checker), _fw_server(_node, _fileserver_backend) +#endif + { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -154,7 +161,10 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); + +#if defined(USE_FW_NODE_SERVER) delete(_server_instance); +#endif } @@ -305,7 +315,7 @@ int UavcanNode::init(uavcan::NodeID node_id) br = br->getSibling(); } - +#if defined(USE_FW_NODE_SERVER) /* Initialize the fw version checker. * giving it it's path */ @@ -373,6 +383,7 @@ int UavcanNode::init(uavcan::NodeID node_id) return ret; } +#endif /* Start the Node */ return _node.start(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 30d0a363b7..43d82082b4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,6 +34,7 @@ #pragma once #include + #include #include #include @@ -47,13 +48,13 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" - -#include -#include -#include -#include -#include - +#if defined(USE_FW_NODE_SERVER) +# include +# include +# include +# include +# include +#endif /** * @file uavcan_main.hpp @@ -147,7 +148,6 @@ private: unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer - static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer Node _node; ///< library instance pthread_mutex_t _node_mutex; @@ -155,11 +155,14 @@ private: UavcanEscController _esc_controller; +#if defined(USE_FW_NODE_SERVER) + static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer + uavcan_posix::BasicFileSeverBackend _fileserver_backend; uavcan::NodeInfoRetriever _node_info_retriever; uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; uavcan::BasicFileServer _fw_server; - +#endif List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; From f6afa23d04f1c708be9664bcfc9e4bb1ed72903d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jun 2015 19:40:57 +0200 Subject: [PATCH 057/493] Fix up SK450 default gains to more reasonable values --- ROMFS/px4fmu_common/init.d/10019_sk450_deadcat | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index c6861c2d45..b91de228c5 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -10,13 +10,13 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 6.0 - param set MC_ROLLRATE_P 0.04 - param set MC_ROLLRATE_I 0.1 + param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_I 0.03 param set MC_ROLLRATE_D 0.0015 param set MC_PITCH_P 6.0 - param set MC_PITCHRATE_P 0.08 - param set MC_PITCHRATE_I 0.2 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.03 param set MC_PITCHRATE_D 0.0015 param set MC_YAW_P 2.8 From 1a8703ec1c0aee86aa2440fc8b7cd627f65854a9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 17 Jun 2015 13:28:27 -0700 Subject: [PATCH 058/493] Improved logging with both compile and runtime level filtering The device level debug will have to be removed and the debugging can be based on this new logging structure which can tell where an error (or debug output) occured whch the current implmentation cannot. The one limitation is the new macros cannot take a char* for the format parameter. It must be an actual string literal because it is concatenated with other strings. Signed-off-by: Mark Charlebois --- src/drivers/device/device_posix.cpp | 17 +- .../commander/state_machine_helper.cpp | 7 +- src/platforms/common/module.mk | 3 +- src/platforms/common/px4_log.c | 5 + src/platforms/px4_log.h | 178 +++++++++++------- 5 files changed, 129 insertions(+), 81 deletions(-) create mode 100644 src/platforms/common/px4_log.c diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index 7d83842266..771bee2471 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -85,11 +85,10 @@ Device::log(const char *fmt, ...) PX4_INFO("[%s] ", _name); va_start(ap, fmt); - PX4_INFO( fmt, ap ); - //vprintf(fmt, ap); + vprintf(fmt, ap); va_end(ap); - //printf("\n"); - //fflush(stdout); + printf("\n"); + fflush(stdout); } void @@ -98,14 +97,12 @@ Device::debug(const char *fmt, ...) va_list ap; if (_debug_enabled) { - PX4_INFO("<%s> ", _name); - //printf("<%s> ", _name); + printf("<%s> ", _name); va_start(ap, fmt); - //vprintf(fmt, ap); - PX4_INFO(fmt, ap); + vprintf(fmt, ap); va_end(ap); - //printf("\n"); - //fflush(stdout); + printf("\n"); + fflush(stdout); } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8d49c305e1..ba5dee9be9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -269,14 +269,15 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } if (ret == TRANSITION_DENIED) { - const char * str = "INVAL: %s - %s"; +#define WARNSTR "INVAL: %s - %s" /* only print to console here by default as this is too technical to be useful during operation */ - warnx(str, state_names[status->arming_state], state_names[new_arming_state]); + warnx(WARNSTR, state_names[status->arming_state], state_names[new_arming_state]); /* print to MAVLink if we didn't provide any feedback yet */ if (!feedback_provided) { - mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); + mavlink_log_critical(mavlink_fd, WARNSTR, state_names[status->arming_state], state_names[new_arming_state]); } +#undef WARNSTR } return ret; diff --git a/src/platforms/common/module.mk b/src/platforms/common/module.mk index 0e5f463e69..472dc7dba6 100644 --- a/src/platforms/common/module.mk +++ b/src/platforms/common/module.mk @@ -2,5 +2,6 @@ # Common OS porting APIs # -SRCS = px4_getopt.c +SRCS = px4_getopt.c \ + px4_log.c diff --git a/src/platforms/common/px4_log.c b/src/platforms/common/px4_log.c new file mode 100644 index 0000000000..0ee4c2a6ad --- /dev/null +++ b/src/platforms/common/px4_log.c @@ -0,0 +1,5 @@ +#include + +__EXPORT unsigned int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; + +__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_DEBUG+1] = { "INFO", "PANIC", "ERROR", "WARN", "DEBUG" }; diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 5576025362..57dbf3e503 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -32,92 +32,136 @@ ****************************************************************************/ /** - * @file px4_log.h - * Platform dependant logging/debug + * @file px4_log_os_impl.h + * Platform dependant logging/debug implementation */ #pragma once -#define __px4_log_omit(level, ...) { } - -#define __px4_log(level, ...) { \ - printf("%-5s ", level);\ - printf(__VA_ARGS__);\ - printf("\n");\ -} -#define __px4_log_verbose(level, ...) { \ - printf("%-5s ", level);\ - printf(__VA_ARGS__);\ - printf(" (file %s line %d)\n", __FILE__, __LINE__);\ -} -#if defined(__PX4_QURT) +#define __STDC_FORMAT_MACROS +#include +#include +#include #include -#define FARF printf -#define __FARF_omit(level, ...) { } -#define __FARF_log(level, ...) { \ - FARF("%-5s ", level);\ - FARF(__VA_ARGS__);\ - FARF("\n");\ -} -#define __FARF_log_verbose(level, ...) { \ - FARF("%-5s ", level);\ - FARF(__VA_ARGS__);\ - FARF(" (file %s line %d)\n", __FILE__, __LINE__);\ -} +__BEGIN_DECLS +__EXPORT extern uint64_t hrt_absolute_time(void); +//__EXPORT extern unsigned long pthread_self(); -#define PX4_DEBUG(...) __FARF_omit("DEBUG", __VA_ARGS__) -#define PX4_INFO(...) __FARF_log("INFO", __VA_ARGS__) -#define PX4_WARN(...) __FARF_log_verbose("WARN", __VA_ARGS__) -#define PX4_ERR(...) __FARF_log_verbose("ERROR", __VA_ARGS__) +#define _PX4_LOG_LEVEL_ALWAYS 0 +#define _PX4_LOG_LEVEL_PANIC 1 +#define _PX4_LOG_LEVEL_ERROR 2 +#define _PX4_LOG_LEVEL_WARN 3 +#define _PX4_LOG_LEVEL_DEBUG 4 -#elif defined(__PX4_LINUX) -#include -#include +extern const char *__px4_log_level_str[5]; +extern unsigned int __px4_log_level_current; -#define __px4_log_threads(level, ...) { \ - printf("%-5s %ld ", level, pthread_self());\ - printf(__VA_ARGS__);\ - printf(" (file %s line %d)\n", __FILE__, __LINE__);\ -} +#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN -#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__) -#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__) -#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__) -#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__) +#define _PX4_LOG_LEVEL_STR(level) __px4_log_level_str[level]; -#elif defined(__PX4_DARWIN) -#include -#include +/**************************************************************************** + * Implementation of log section formatting based on printf + ****************************************************************************/ +#if defined(__PX4_ROS) +#define __px4__log_startline(level) if (level <= __px4_log_level_current) ROS_WARN( +#else +#define __px4__log_startline(level) if (level <= __px4_log_level_current) printf( +#endif +#define __px4__log_timestamp_fmt "%-10" PRIu64 +#define __px4__log_timestamp_arg ,hrt_absolute_time() +#define __px4__log_level_fmt "%-5s " +#define __px4__log_level_arg(level) ,__px4_log_level_str[level] +#define __px4__log_thread_fmt "%ld " +#define __px4__log_thread_arg ,pthread_self() -#define __px4_log_threads(level, ...) { \ - printf("%-5s %ld ", level, pthread_self());\ - printf(__VA_ARGS__);\ - printf(" (file %s line %d)\n", __FILE__, __LINE__);\ -} +#define __px4__log_file_and_line_fmt " (file %s line %d)" +#define __px4__log_file_and_line_arg , __FILE__, __LINE__ +#define __px4__log_end_fmt "\n" +#define __px4__log_endline ) -#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__) -#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__) -#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__) -#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__) +/**************************************************************************** + * Output format macros + * Use these to implement the code level macros below + ****************************************************************************/ +#define __px4_log_omit(level, FMT, ...) { } -#elif defined(__PX4_ROS) +#define __px4_log(level, FMT, ...) \ + __px4__log_startline(level)\ + __px4__log_level_fmt \ + FMT\ + __px4__log_end_fmt \ + __px4__log_level_arg(level), ##__VA_ARGS__\ + __px4__log_endline -#define PX4_DBG(...) -#define PX4_INFO(...) ROS_WARN(__VA_ARGS__) -#define PX4_WARN(...) ROS_WARN(__VA_ARGS__) -#define PX4_ERR(...) ROS_WARN(__VA_ARGS__) +#define __px4_log_timestamp(level, FMT, ...) \ + __px4__log_startline(level)\ + __px4__log_timestamp_fmt\ + __px4__log_level_fmt\ + FMT\ + __px4__log_end_fmt\ + __px4__log_timestamp_arg\ + __px4__log_level_arg(level), ##__VA_ARGS__\ + __px4__log_endline -#elif defined(__PX4_NUTTX) -#include +#define __px4_log_file_and_line(level, FMT, ...) \ + __px4__log_startline(level)\ + __px4__log_timestamp_fmt\ + __px4__log_level_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_timestamp_arg\ + __px4__log_level_arg(level), ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + __px4__log_endline -#define PX4_DBG(...) -#define PX4_INFO(...) warnx(__VA_ARGS__) -#define PX4_WARN(...) warnx(__VA_ARGS__) -#define PX4_ERR(...) warnx(__VA_ARGS__) +#define __px4_log_timestamp_file_and_line(level, FMT, ...) \ + __px4__log_startline(level)\ + __px4__log_timestamp_fmt\ + __px4__log_level_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_timestamp_arg\ + __px4__log_level_arg(level) , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + __px4__log_endline + +#define __px4_log_thread_file_and_line(level, FMT, ...) \ + __px4__log_startline(level)\ + __px4__log_thread_fmt\ + __px4__log_level_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_thread_arg\ + __px4__log_level_arg(level) , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + __px4__log_endline + + +/**************************************************************************** + * Code level macros + * These are the log APIs that should be used by the code + ****************************************************************************/ +#define PX4_LOG(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, __VA_ARGS__) + +#if defined(DEBUG_BUILD) + +#define PX4_PANIC(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_timestamp(_PX4_LOG_LEVEL_DEBUG, FMT, __VA_ARGS__) #else -#error "Target platform unknown" +#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) __px4_log(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #endif +__END_DECLS From 65e9fd9dd8df747541d2d3ee44fc88b3c7b90c90 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 17 Jun 2015 13:37:27 -0700 Subject: [PATCH 059/493] px4_log: minor fixes to logging header file Signed-off-by: Mark Charlebois --- src/platforms/px4_log.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 57dbf3e503..d18026b07e 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4_log_os_impl.h + * @file px4_log.h * Platform dependant logging/debug implementation */ @@ -46,7 +46,6 @@ __BEGIN_DECLS __EXPORT extern uint64_t hrt_absolute_time(void); -//__EXPORT extern unsigned long pthread_self(); #define _PX4_LOG_LEVEL_ALWAYS 0 #define _PX4_LOG_LEVEL_PANIC 1 @@ -54,8 +53,8 @@ __EXPORT extern uint64_t hrt_absolute_time(void); #define _PX4_LOG_LEVEL_WARN 3 #define _PX4_LOG_LEVEL_DEBUG 4 -extern const char *__px4_log_level_str[5]; -extern unsigned int __px4_log_level_current; +__EXPORT extern const char *__px4_log_level_str[5]; +__EXPORT extern unsigned int __px4_log_level_current; #define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN From 959333d6cc8e5f99ee68b2dff9cb65d54d805985 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jun 2015 22:44:51 +0200 Subject: [PATCH 060/493] Re-balance FMUv2 config in terms of buffer sizes to free some excessively used resources --- nuttx-configs/px4fmu-v2/nsh/defconfig | 33 ++++++++++++++------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 19e0f7c632..ef9c673785 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -367,7 +367,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=1500 +# The actual usage is 420 bytes +CONFIG_ARCH_INTERRUPTSTACK=750 # # Boot options @@ -548,8 +549,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=512 -CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=600 CONFIG_USART1_BAUD=115200 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 @@ -561,7 +562,7 @@ CONFIG_USART1_2STOP=0 # USART2 Configuration # CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=2200 +CONFIG_USART2_TXBUFSIZE=1860 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 @@ -572,8 +573,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # USART3 Configuration # -CONFIG_USART3_RXBUFSIZE=512 -CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_RXBUFSIZE=400 +CONFIG_USART3_TXBUFSIZE=400 CONFIG_USART3_BAUD=57600 CONFIG_USART3_BITS=8 CONFIG_USART3_PARITY=0 @@ -584,8 +585,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=512 -CONFIG_UART4_TXBUFSIZE=512 +CONFIG_UART4_RXBUFSIZE=400 +CONFIG_UART4_TXBUFSIZE=400 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -596,8 +597,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=512 -CONFIG_USART6_TXBUFSIZE=512 +CONFIG_USART6_RXBUFSIZE=400 +CONFIG_USART6_TXBUFSIZE=400 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -608,8 +609,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=512 -CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_RXBUFSIZE=400 +CONFIG_UART7_TXBUFSIZE=400 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -620,8 +621,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=512 -CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_RXBUFSIZE=400 +CONFIG_UART8_TXBUFSIZE=400 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 @@ -663,8 +664,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=1000 -CONFIG_CDCACM_TXBUFSIZE=8000 +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=5000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From a2297aa950c6adf499fa8f225e9702e4b342c34c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 17 Jun 2015 13:49:34 -0700 Subject: [PATCH 061/493] px4_log: Fixed ROS build Signed-off-by: Mark Charlebois --- src/platforms/px4_log.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index d18026b07e..91107b5490 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -44,6 +44,16 @@ #include #include +#if defined(__PX4_ROS) + +#define PX4_PANIC(...) ROS_WARN(__VA_ARGS__) +#define PX4_ERR(...) ROS_WARN(__VA_ARGS__) +#define PX4_WARN(...) ROS_WARN(__VA_ARGS__) +#define PX4_INFO(...) ROS_WARN(__VA_ARGS__) +#define PX4_DEBUG(...) + +#else + __BEGIN_DECLS __EXPORT extern uint64_t hrt_absolute_time(void); @@ -164,3 +174,4 @@ __EXPORT extern unsigned int __px4_log_level_current; #endif __END_DECLS +#endif From dad0526a9975a5fb053484815bf332b58c1410a6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 17 Jun 2015 13:50:49 -0700 Subject: [PATCH 062/493] px4_log: Added include for ROS Signed-off-by: Mark Charlebois --- src/platforms/px4_log.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 91107b5490..7e89dbe11d 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -46,6 +46,7 @@ #if defined(__PX4_ROS) +#include #define PX4_PANIC(...) ROS_WARN(__VA_ARGS__) #define PX4_ERR(...) ROS_WARN(__VA_ARGS__) #define PX4_WARN(...) ROS_WARN(__VA_ARGS__) From 29a36da22c8c5e79b46e6a454538fbc982ab05cc Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 17 Jun 2015 17:11:21 -0700 Subject: [PATCH 063/493] px4_log: Added documentation and handled unused variables Added __attribute__ ((unused)) for variables used only for log output and flagged as unused if the message log level is compiled out. Signed-off-by: Mark Charlebois --- src/modules/commander/commander.cpp | 2 +- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sensors/sensors.cpp | 2 +- src/platforms/px4_log.h | 178 +++++++++++++++++++++++----- 4 files changed, 155 insertions(+), 31 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d47b45d89f..38a547282f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -361,7 +361,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); - int checkres = prearm_check(&status, mavlink_fd_local); + int checkres __attribute__ ((unused)) = prearm_check(&status, mavlink_fd_local); close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 270804075e..67e45154cb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1936,8 +1936,8 @@ void sdlog2_status() } else { float kibibytes = log_bytes_written / 1024.0f; - float mebibytes = kibibytes / 1024.0f; - float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; + float mebibytes __attribute__ ((unused)) = kibibytes / 1024.0f; + float seconds __attribute__ ((unused)) = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0be31046b1..484092c747 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -706,7 +706,7 @@ Sensors::parameters_update() warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); } - const char *paramerr = "FAIL PARM LOAD"; + const char *paramerr __attribute__ ((unused)) = "FAIL PARM LOAD"; /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 7e89dbe11d..d52582f15e 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -38,12 +38,6 @@ #pragma once -#define __STDC_FORMAT_MACROS -#include -#include -#include -#include - #if defined(__PX4_ROS) #include @@ -55,6 +49,12 @@ #else +#define __STDC_FORMAT_MACROS +#include +#include +#include +#include + __BEGIN_DECLS __EXPORT extern uint64_t hrt_absolute_time(void); @@ -67,26 +67,29 @@ __EXPORT extern uint64_t hrt_absolute_time(void); __EXPORT extern const char *__px4_log_level_str[5]; __EXPORT extern unsigned int __px4_log_level_current; +// __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME #define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN -#define _PX4_LOG_LEVEL_STR(level) __px4_log_level_str[level]; - /**************************************************************************** * Implementation of log section formatting based on printf + * + * To write to a specific stream for each message type, open the streams and + * set __px4__log_startline to something like: + * if (level <= __px4_log_level_current) printf(_px4_fd[level], + * + * Additional behavior can be added using "{\" for __px4__log_startline and + * "}" for __px4__log_endline and any other required setup or teardown steps ****************************************************************************/ -#if defined(__PX4_ROS) -#define __px4__log_startline(level) if (level <= __px4_log_level_current) ROS_WARN( -#else #define __px4__log_startline(level) if (level <= __px4_log_level_current) printf( -#endif -#define __px4__log_timestamp_fmt "%-10" PRIu64 + +#define __px4__log_timestamp_fmt "%-10" PRIu64 " " #define __px4__log_timestamp_arg ,hrt_absolute_time() #define __px4__log_level_fmt "%-5s " #define __px4__log_level_arg(level) ,__px4_log_level_str[level] -#define __px4__log_thread_fmt "%ld " +#define __px4__log_thread_fmt "%#X " #define __px4__log_thread_arg ,pthread_self() -#define __px4__log_file_and_line_fmt " (file %s line %d)" +#define __px4__log_file_and_line_fmt " (file %s line %u)" #define __px4__log_file_and_line_arg , __FILE__, __LINE__ #define __px4__log_end_fmt "\n" #define __px4__log_endline ) @@ -95,8 +98,20 @@ __EXPORT extern unsigned int __px4_log_level_current; * Output format macros * Use these to implement the code level macros below ****************************************************************************/ + +/**************************************************************************** + * __px4_log_omit: + * Compile out the message + ****************************************************************************/ #define __px4_log_omit(level, FMT, ...) { } +/**************************************************************************** + * __px4_log: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s val is %d\n", __px4_log_level_str[3], val); + ****************************************************************************/ #define __px4_log(level, FMT, ...) \ __px4__log_startline(level)\ __px4__log_level_fmt \ @@ -105,49 +120,132 @@ __EXPORT extern unsigned int __px4_log_level_current; __px4__log_level_arg(level), ##__VA_ARGS__\ __px4__log_endline +/**************************************************************************** + * __px4_log_timestamp: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %10lu val is %d\n", __px4_log_level_str[3], + * hrt_absolute_time(), val); + ****************************************************************************/ #define __px4_log_timestamp(level, FMT, ...) \ __px4__log_startline(level)\ - __px4__log_timestamp_fmt\ __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ FMT\ __px4__log_end_fmt\ + __px4__log_level_arg(level)\ __px4__log_timestamp_arg\ - __px4__log_level_arg(level), ##__VA_ARGS__\ + , ##__VA_ARGS__\ __px4__log_endline +/**************************************************************************** + * __px4_log_timestamp_thread: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %10lu %#X val is %d\n", __px4_log_level_str[3], + * hrt_absolute_time(), pthread_self(), val); + ****************************************************************************/ +#define __px4_log_timestamp_thread(level, FMT, ...) \ + __px4__log_startline(level)\ + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + __px4__log_endline + +/**************************************************************************** + * __px4_log_file_and_line: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s val is %d (file %s line %u)\n", + * __px4_log_level_str[3], val, __FILE__, __LINE__); + ****************************************************************************/ #define __px4_log_file_and_line(level, FMT, ...) \ __px4__log_startline(level)\ - __px4__log_timestamp_fmt\ __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ FMT\ __px4__log_file_and_line_fmt\ __px4__log_end_fmt\ + __px4__log_level_arg(level)\ __px4__log_timestamp_arg\ - __px4__log_level_arg(level), ##__VA_ARGS__\ + , ##__VA_ARGS__\ __px4__log_file_and_line_arg\ __px4__log_endline +/**************************************************************************** + * __px4_log_timestamp_file_and_line: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %-10lu val is %d (file %s line %u)\n", + * __px4_log_level_str[3], hrt_absolute_time(), + * val, __FILE__, __LINE__); + ****************************************************************************/ #define __px4_log_timestamp_file_and_line(level, FMT, ...) \ __px4__log_startline(level)\ - __px4__log_timestamp_fmt\ __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ FMT\ __px4__log_file_and_line_fmt\ __px4__log_end_fmt\ + __px4__log_level_arg(level)\ __px4__log_timestamp_arg\ - __px4__log_level_arg(level) , ##__VA_ARGS__\ + , ##__VA_ARGS__\ __px4__log_file_and_line_arg\ __px4__log_endline +/**************************************************************************** + * __px4_log_thread_file_and_line: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %#X val is %d (file %s line %u)\n", + * __px4_log_level_str[3], pthread_self(), + * val, __FILE__, __LINE__); + ****************************************************************************/ #define __px4_log_thread_file_and_line(level, FMT, ...) \ __px4__log_startline(level)\ - __px4__log_thread_fmt\ __px4__log_level_fmt\ + __px4__log_thread_fmt\ FMT\ __px4__log_file_and_line_fmt\ __px4__log_end_fmt\ + __px4__log_level_arg(level)\ __px4__log_thread_arg\ - __px4__log_level_arg(level) , ##__VA_ARGS__\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + __px4__log_endline + +/**************************************************************************** + * __px4_log_timestamp_thread_file_and_line: + * Convert a message in the form: + * PX4_WARN("val is %d", val); + * to + * printf("%-5s %-10lu %#X val is %d (file %s line %u)\n", + * __px4_log_level_str[3], hrt_absolute_time(), + * pthread_self(), val, __FILE__, __LINE__); + ****************************************************************************/ +#define __px4_log_timestamp_thread_file_and_line(level, FMT, ...) \ + __px4__log_startline(level)\ + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ __px4__log_file_and_line_arg\ __px4__log_endline @@ -156,21 +254,47 @@ __EXPORT extern unsigned int __px4_log_level_current; * Code level macros * These are the log APIs that should be used by the code ****************************************************************************/ -#define PX4_LOG(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, __VA_ARGS__) -#if defined(DEBUG_BUILD) +/**************************************************************************** + * Messages that should never be filtered or compiled out + ****************************************************************************/ +#define PX4_LOG(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__) +#if defined(TRACE_BUILD) +/**************************************************************************** + * Extremely Verbose settings for a Trace build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) __px4_log_timestamp_thread_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log_timestamp_thread_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_timestamp_thread_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_timestamp_thread(_PX4_LOG_LEVEL_DEBUG, FMT, __VA_ARGS__) + +#elif defined(DEBUG_BUILD) +/**************************************************************************** + * Verbose settings for a Debug build + ****************************************************************************/ #define PX4_PANIC(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) #define PX4_ERR(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) #define PX4_WARN(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) #define PX4_DEBUG(FMT, ...) __px4_log_timestamp(_PX4_LOG_LEVEL_DEBUG, FMT, __VA_ARGS__) -#else +#elif defined(RELEASE_BUILD) +/**************************************************************************** + * Non-verbose settings for a Release build to minimize strings in build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) +#else +/**************************************************************************** + * Medium verbose settings for a default build + ****************************************************************************/ #define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) #define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) #define PX4_WARN(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) -#define PX4_INFO(FMT, ...) __px4_log(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #endif From fc5eb7af6f2e30243835beac68e21b91ab319915 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 17 Jun 2015 18:05:04 -0700 Subject: [PATCH 064/493] unittests: Fixed dependency on px4_log.c px4_log.c was added to px4_platform library and the library was added to unit tests that use the log macros. There is also a dependency on hrt_absolute_time() as well which requires px4_platform. Signed-off-by: Mark Charlebois --- unittests/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index b4e505209d..580d9e5d81 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -75,6 +75,7 @@ endfunction() add_library( px4_platform # ${PX_SRC}/platforms/common/px4_getopt.c + ${PX_SRC}/platforms/common/px4_log.c ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp ${PX_SRC}/platforms/posix/px4_layer/work_lock.c @@ -130,22 +131,27 @@ add_gtest(mixer_test) # conversion_test add_executable(conversion_test conversion_test.cpp ${PX_SRC}/systemcmds/tests/test_conv.cpp) +target_link_libraries( conversion_test px4_platform ) add_gtest(conversion_test) # sbus2_test add_executable(sbus2_test sbus2_test.cpp hrt.cpp) +target_link_libraries( sbus2_test px4_platform ) add_gtest(sbus2_test) # st24_test add_executable(st24_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c) +target_link_libraries( st24_test px4_platform ) add_gtest(st24_test) # sumd_test add_executable(sumd_test sumd_test.cpp hrt.cpp ${PX_SRC}/lib/rc/sumd.c) +target_link_libraries( sumd_test px4_platform ) add_gtest(sumd_test) # sf0x_test add_executable(sf0x_test sf0x_test.cpp ${PX_SRC}/drivers/sf0x/sf0x_parser.cpp) +target_link_libraries( sf0x_test px4_platform ) add_gtest(sf0x_test) # param_test From 552c9800a9a394e5ad351309d62278aecd44073f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 17 Jun 2015 19:04:57 -0700 Subject: [PATCH 065/493] px4_log: Fixed compiler warning when using PX4_LOG If __px4_log_level_current is unsigned then the runtime filter comparison warns because an unsigned value can't be less than zero. Changed typed to signed so compiler will not issue a warning. Signed-off-by: Mark Charlebois --- src/platforms/common/px4_log.c | 2 +- src/platforms/px4_log.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/common/px4_log.c b/src/platforms/common/px4_log.c index 0ee4c2a6ad..a2c61ab297 100644 --- a/src/platforms/common/px4_log.c +++ b/src/platforms/common/px4_log.c @@ -1,5 +1,5 @@ #include -__EXPORT unsigned int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; +__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_DEBUG+1] = { "INFO", "PANIC", "ERROR", "WARN", "DEBUG" }; diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index d52582f15e..05278131c8 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -65,7 +65,7 @@ __EXPORT extern uint64_t hrt_absolute_time(void); #define _PX4_LOG_LEVEL_DEBUG 4 __EXPORT extern const char *__px4_log_level_str[5]; -__EXPORT extern unsigned int __px4_log_level_current; +__EXPORT extern int __px4_log_level_current; // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME #define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN From 3cd211ed72c39a9d9afa392138aff2060a4e2d41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jun 2015 08:55:34 +0200 Subject: [PATCH 066/493] MC pos control: Do not raise min throttle too far. --- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4a58dfdd0c..4865c1c684 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -50,7 +50,7 @@ * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.18f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); /** * Maximum thrust From a94a8c5f5163ba91452f34a0a1008b4d8841427d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jun 2015 08:56:36 +0200 Subject: [PATCH 067/493] sdlog2: Flow: Remove unused field --- src/modules/sdlog2/sdlog2_messages.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index abdf518c51..e75b6ca256 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -218,7 +218,6 @@ struct log_ARSP_s { /* --- FLOW - OPTICAL FLOW --- */ #define LOG_FLOW_MSG 15 struct log_FLOW_s { - uint64_t timestamp; uint8_t sensor_id; float pixel_flow_x_integral; float pixel_flow_y_integral; @@ -508,7 +507,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "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, "QBffffffLLHhB", "IntT,ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), + LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), From e08dc0df4071ec2b2a6406d16bbc1e502cb2afb4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jun 2015 11:03:32 +0200 Subject: [PATCH 068/493] Add support for RC_CHANNELS_OVERRIDE in addition to normal message --- src/modules/mavlink/mavlink_receiver.cpp | 46 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 47 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1a6494ad93..8ebbe47052 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -194,6 +194,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + handle_message_rc_channels_override(msg); + break; + case MAVLINK_MSG_ID_HEARTBEAT: handle_message_heartbeat(msg); break; @@ -912,6 +916,48 @@ static int decode_switch_pos_n(uint16_t buttons, int sw) { } } +void +MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) +{ + mavlink_rc_channels_override_t man; + mavlink_msg_rc_channels_override_decode(msg, &man); + + // Check target + if (man.target_system != 0 && man.target_system != _mavlink->get_system_id()) { + return; + } + + struct rc_input_values rc = {}; + rc.timestamp_publication = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp_publication; + + rc.channel_count = 8; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = 0; + rc.input_source = RC_INPUT_SOURCE_MAVLINK; + rc.rssi = RC_INPUT_RSSI_MAX; + + /* channels */ + rc.values[0] = man.chan1_raw; + rc.values[1] = man.chan2_raw; + rc.values[2] = man.chan3_raw; + rc.values[3] = man.chan4_raw; + rc.values[4] = man.chan5_raw; + rc.values[5] = man.chan6_raw; + rc.values[6] = man.chan7_raw; + rc.values[7] = man.chan8_raw; + + if (_rc_pub <= 0) { + _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); + + } else { + orb_publish(ORB_ID(input_rc), _rc_pub, &rc); + } +} + void MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fe217f3c3b..18a3dc208a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -126,6 +126,7 @@ private: void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_rc_channels_override(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); void handle_message_request_data_stream(mavlink_message_t *msg); From 785053e4f100cde01a32b29133918e0ba150b115 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 18 Jun 2015 07:41:22 -0700 Subject: [PATCH 069/493] px4_log: reverted unused attribute annotations Used a do_nothing() function for px4_omit() that will satisfy the compiler so it will not report unused variables when a debug message is compiled out. Signed-off-by: Mark Charlebois --- src/modules/commander/commander.cpp | 2 +- src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/sensors/sensors.cpp | 2 +- src/platforms/px4_log.h | 8 +++++++- 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 38a547282f..d47b45d89f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -361,7 +361,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); - int checkres __attribute__ ((unused)) = prearm_check(&status, mavlink_fd_local); + int checkres = prearm_check(&status, mavlink_fd_local); close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 67e45154cb..270804075e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1936,8 +1936,8 @@ void sdlog2_status() } else { float kibibytes = log_bytes_written / 1024.0f; - float mebibytes __attribute__ ((unused)) = kibibytes / 1024.0f; - float seconds __attribute__ ((unused)) = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; + float mebibytes = kibibytes / 1024.0f; + float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 484092c747..0be31046b1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -706,7 +706,7 @@ Sensors::parameters_update() warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); } - const char *paramerr __attribute__ ((unused)) = "FAIL PARM LOAD"; + const char *paramerr = "FAIL PARM LOAD"; /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 05278131c8..327df7abe1 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -58,6 +58,12 @@ __BEGIN_DECLS __EXPORT extern uint64_t hrt_absolute_time(void); +// Used to silence unused variable warning +static inline void do_nothing(int level, ...) +{ + (void)level; +} + #define _PX4_LOG_LEVEL_ALWAYS 0 #define _PX4_LOG_LEVEL_PANIC 1 #define _PX4_LOG_LEVEL_ERROR 2 @@ -103,7 +109,7 @@ __EXPORT extern int __px4_log_level_current; * __px4_log_omit: * Compile out the message ****************************************************************************/ -#define __px4_log_omit(level, FMT, ...) { } +#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__) /**************************************************************************** * __px4_log: From a65264228670419738452ae270cba8700987aca7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jun 2015 14:24:35 -0700 Subject: [PATCH 070/493] POSIX: Fix dataman start order --- posix-configs/SITL/init/rcS | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 9ad8db7f7d..d2057d4b03 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -1,6 +1,7 @@ uorb start param load -mavlink start -u 14556 +dataman start +mavlink start -u 14556 -r 60000 simulator start -s param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1310720 @@ -19,8 +20,6 @@ ekf_att_pos_estimator start mc_pos_control start mc_att_control start hil mode_pwm -dataman start -navigator start param set MAV_TYPE 2 param set RC1_MAX 2015 param set RC1_MIN 996 From 2485e037943222b521b6cc98eb6bcf50c82a3fbe Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:54:58 +0200 Subject: [PATCH 071/493] corrected elevon mixer for firefly6 --- ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index fda8416403..22dc2a69ce 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -11,12 +11,12 @@ Elevon mixers ------------- M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 Landing gear mixer From 1ccded0305f439b4f8e45f23ec55bf304cc7c1ab Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:55:30 +0200 Subject: [PATCH 072/493] added generic class for vtol types --- src/modules/vtol_att_control/vtol_type.cpp | 133 +++++++++++++++++++++ src/modules/vtol_att_control/vtol_type.h | 115 ++++++++++++++++++ 2 files changed, 248 insertions(+) create mode 100644 src/modules/vtol_att_control/vtol_type.cpp create mode 100644 src/modules/vtol_att_control/vtol_type.h diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp new file mode 100644 index 0000000000..7e8d3217f1 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + /** + * @file airframe.cpp + * + * @author Roman Bapst + * + */ + +#include "vtol_type.h" +#include "drivers/drv_pwm_output.h" +#include +#include "vtol_att_control_main.h" + +VtolType::VtolType(VtolAttitudeControl *att_controller) : +_attc(att_controller), +_vtol_mode(ROTARY_WING) +{ + _v_att = _attc->get_att(); + _v_att_sp = _attc->get_att_sp(); + _v_rates_sp = _attc->get_rates_sp(); + _mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp(); + _fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp(); + _manual_control_sp = _attc->get_manual_control_sp(); + _v_control_mode = _attc->get_control_mode(); + _vtol_vehicle_status = _attc->get_vehicle_status(); + _actuators_out_0 = _attc->get_actuators_out0(); + _actuators_out_1 = _attc->get_actuators_out1(); + _actuators_mc_in = _attc->get_actuators_mc_in(); + _actuators_fw_in = _attc->get_actuators_fw_in(); + _armed = _attc->get_armed(); + _local_pos = _attc->get_local_pos(); + _airspeed = _attc->get_airspeed(); + _batt_status = _attc->get_batt_status(); + _params = _attc->get_params(); + + flag_idle_mc = true; +} + +VtolType::~VtolType() +{ + +} + +/** +* Adjust idle speed for mc mode. +*/ +void VtolType::set_idle_mc() +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + unsigned pwm_value = _params->idle_pwm_mc; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); + + flag_idle_mc = true; +} + +/** +* Adjust idle speed for fw mode. +*/ +void VtolType::set_idle_fw() +{ + int ret; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + unsigned pwm_value = PWM_LOWEST_MIN; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); +} \ No newline at end of file diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h new file mode 100644 index 0000000000..57448a7587 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_type.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + /** + * @file airframe.h + * + * @author Roman Bapst + * + */ + +#ifndef VTOL_YYPE_H +#define VTOL_YYPE_H + +struct Params { + int idle_pwm_mc; // pwm value for idle in mc mode + int vtol_motor_count; // number of motors + int vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode + float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) + float mc_airspeed_trim; // trim airspeed in multicopter mode + float mc_airspeed_max; // max airpseed in multicopter mode + float fw_pitch_trim; // trim for neutral elevon position in fw mode + float power_max; // maximum power of one engine + float prop_eff; // factor to calculate prop efficiency + float arsp_lp_gain; // total airspeed estimate low pass gain + int vtol_type; +}; + +enum mode { + ROTARY_WING = 0, + FIXED_WING, + TRANSITION, + EXTERNAL +}; + +class VtolAttitudeControl; + +class VtolType +{ +public: + + VtolType(VtolAttitudeControl *att_controller); + + virtual ~VtolType(); + + virtual void update_vtol_state() = 0; + virtual void update_mc_state() = 0; + virtual void process_mc_data() = 0; + virtual void update_fw_state() = 0; + virtual void process_fw_data() = 0; + virtual void update_transition_state() = 0; + virtual void update_external_state() = 0; + + void set_idle_mc(); + void set_idle_fw(); + + mode get_mode () {return _vtol_mode;}; + +protected: + VtolAttitudeControl *_attc; + mode _vtol_mode; + + struct vehicle_attitude_s *_v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s *_vtol_vehicle_status; + struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s *_armed; //actuator arming status + struct vehicle_local_position_s *_local_pos; + struct airspeed_s *_airspeed; // airspeed + struct battery_status_s *_batt_status; // battery status + + struct Params *_params; + + bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + +}; + +#endif From a212e457448ed41c8a33c39696a8963ce631f63d Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:56:11 +0200 Subject: [PATCH 073/493] added tiltrotor attitude control class --- src/modules/vtol_att_control/tiltrotor.cpp | 357 ++++++++++++++++++ src/modules/vtol_att_control/tiltrotor.h | 110 ++++++ .../vtol_att_control/tiltrotor_params.c | 118 ++++++ 3 files changed, 585 insertions(+) create mode 100644 src/modules/vtol_att_control/tiltrotor.cpp create mode 100644 src/modules/vtol_att_control/tiltrotor.h create mode 100644 src/modules/vtol_att_control/tiltrotor_params.c diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp new file mode 100644 index 0000000000..2cf074fe42 --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -0,0 +1,357 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file tiltrotor.cpp + * + * @author Roman Bapst + * +*/ + +#include "tiltrotor.h" +#include "vtol_att_control_main.h" + +#define ARSP_BLEND_START 8.0f // airspeed at which we start blending mc/fw controls + +Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : +VtolType(attc), +flag_max_mc(true), +_tilt_control(0.0f), +_roll_weight_mc(1.0f) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); + _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); + _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); + _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + } + +Tiltrotor::~Tiltrotor() +{ + +} + +int +Tiltrotor::parameters_update() +{ + float v; + int l; + + /* vtol duration of a front transition */ + param_get(_params_handles_tiltrotor.front_trans_dur, &v); + _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); + + /* vtol duration of a back transition */ + param_get(_params_handles_tiltrotor.back_trans_dur, &v); + _params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f); + + /* vtol tilt mechanism position in mc mode */ + param_get(_params_handles_tiltrotor.tilt_mc, &v); + _params_tiltrotor.tilt_mc = v; + + /* vtol tilt mechanism position in transition mode */ + param_get(_params_handles_tiltrotor.tilt_transition, &v); + _params_tiltrotor.tilt_transition = v; + + /* vtol tilt mechanism position in fw mode */ + param_get(_params_handles_tiltrotor.tilt_fw, &v); + _params_tiltrotor.tilt_fw = v; + + /* vtol airspeed at which it is ok to switch to fw mode */ + param_get(_params_handles_tiltrotor.airspeed_trans, &v); + _params_tiltrotor.airspeed_trans = v; + + /* vtol lock elevons in multicopter */ + param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); + _params_tiltrotor.elevons_mc_lock = l; + + return OK; +} + +void Tiltrotor::update_vtol_state() +{ + parameters_update(); + + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting rotors, picking up + * forward speed. After the vehicle has picked up enough speed the rotors are tilted + * forward completely. For the backtransition the motors simply rotate back. + */ + + if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == MC_MODE) { + // mc mode + _vtol_schedule.flight_mode = MC_MODE; + _tilt_control = _params_tiltrotor.tilt_mc; + _roll_weight_mc = 1.0f; + } else if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == FW_MODE) { + _vtol_schedule.flight_mode = TRANSITION_BACK; + flag_max_mc = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + } else if (_manual_control_sp->aux1 >= 0.0f && _vtol_schedule.flight_mode == MC_MODE) { + // instant of doeing a front-transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 > 0.0f) { + // check if we have reached airspeed to switch to fw mode + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) { + _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; + flag_max_mc = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + } + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 > 0.0f) { + if (_tilt_control >= _params_tiltrotor.tilt_fw) { + _vtol_schedule.flight_mode = FW_MODE; + _tilt_control = _params_tiltrotor.tilt_fw; + } + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 < 0.0f) { + // failsave into mc mode + _vtol_schedule.flight_mode = MC_MODE; + _tilt_control = _params_tiltrotor.tilt_mc; + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 < 0.0f) { + // failsave into mc mode + _vtol_schedule.flight_mode = MC_MODE; + _tilt_control = _params_tiltrotor.tilt_mc; + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 < 0.0f) { + if (_tilt_control <= _params_tiltrotor.tilt_mc) { + _vtol_schedule.flight_mode = MC_MODE; + _tilt_control = _params_tiltrotor.tilt_mc; + flag_max_mc = false; + } + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 > 0.0f) { + // failsave into fw mode + _vtol_schedule.flight_mode = FW_MODE; + _tilt_control = _params_tiltrotor.tilt_fw; + } + + // tilt rotors if necessary + update_transition_state(); + + // map tiltrotor specific control phases to simple control modes + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + case TRANSITION_FRONT_P1: + case TRANSITION_FRONT_P2: + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + break; + } +} + +void Tiltrotor::update_mc_state() +{ + // adjust max pwm for rear motors to spin up + if (!flag_max_mc) { + set_max_mc(); + flag_max_mc = true; + } + + // set idle speed for rotary wing mode + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } +} + +void Tiltrotor::process_mc_data() +{ + fill_mc_att_control_output(); +} + + void Tiltrotor::update_fw_state() +{ + /* in fw mode we need the rear motors to stop spinning, in backtransition + * mode we let them spin in idle + */ + if (flag_max_mc) { + if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + set_max_fw(1200); + set_idle_mc(); + } else { + set_max_fw(950); + set_idle_fw(); + } + flag_max_mc = false; + } + + // adjust idle for fixed wing flight + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } + } + +void Tiltrotor::process_fw_data() +{ + fill_fw_att_control_output(); +} + +void Tiltrotor::update_transition_state() +{ + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + // tilt rotors forward up to certain angle + if (_tilt_control <= _params_tiltrotor.tilt_transition) { + _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f); + } + + // do blending of mc and fw controls + if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) { + _roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START); + } else { + // at low speeds give full weight to mc + _roll_weight_mc = 1.0f; + } + + _roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f); + + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f); + _roll_weight_mc = 0.0f; + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + // tilt rotors forward up to certain angle + float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f); + if (_tilt_control > _params_tiltrotor.tilt_mc) { + _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress; + } + + _roll_weight_mc = progress; + } +} + +void Tiltrotor::update_external_state() +{ + +} + + /** +* Prepare message to acutators with data from mc attitude controller. +*/ +void Tiltrotor::fill_mc_att_control_output() +{ + _actuators_out_0->control[0] = _actuators_mc_in->control[0]; + _actuators_out_0->control[1] = _actuators_mc_in->control[1]; + _actuators_out_0->control[2] = _actuators_mc_in->control[2]; + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; + + _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon + _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon + + _actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control +} + +/** +* Prepare message to acutators with data from fw attitude controller. +*/ +void Tiltrotor::fill_fw_att_control_output() +{ + /*For the first test in fw mode, only use engines for thrust!!!*/ + _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; + _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; + _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; + _actuators_out_0->control[3] = _actuators_fw_in->control[3]; + /*controls for the elevons */ + _actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon + _actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon + // unused now but still logged + _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle + _actuators_out_1->control[4] = _tilt_control; +} + +/** +* Kill rear motors for the FireFLY6 when in fw mode. +*/ +void +Tiltrotor::set_max_fw(unsigned pwm_value) +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + if (i == 2 || i == 3) { + pwm_values.values[i] = pwm_value; + } else { + pwm_values.values[i] = 2000; + } + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting max values");} + + close(fd); +} + +void +Tiltrotor::set_max_mc() +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = 2000; + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting max values");} + + close(fd); +} diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h new file mode 100644 index 0000000000..3ef1362d00 --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + /** + * @file tiltrotor.h + * + * @author Roman Bapst + * + */ + +#ifndef TILTROTOR_H +#define TILTROTOR_H +#include "vtol_type.h" +#include +#include + +class Tiltrotor : public VtolType +{ + +public: + + Tiltrotor(VtolAttitudeControl * _att_controller); + ~Tiltrotor(); + + void update_vtol_state(); + void update_mc_state(); + void process_mc_data(); + void update_fw_state(); + void process_fw_data(); + void update_transition_state(); + void update_external_state(); + +private: + + struct { + float front_trans_dur; + float back_trans_dur; + float tilt_mc; + float tilt_transition; + float tilt_fw; + float airspeed_trans; + int elevons_mc_lock; // lock elevons in multicopter mode + } _params_tiltrotor; + + struct { + param_t front_trans_dur; + param_t back_trans_dur; + param_t tilt_mc; + param_t tilt_transition; + param_t tilt_fw; + param_t airspeed_trans; + param_t elevons_mc_lock; + } _params_handles_tiltrotor; + + enum vtol_mode { + MC_MODE = 0, + TRANSITION_FRONT_P1, + TRANSITION_FRONT_P2, + TRANSITION_BACK, + FW_MODE + }; + + struct { + vtol_mode flight_mode; // indicates in which mode the vehicle is in + hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) + }_vtol_schedule; + + bool flag_max_mc; + float _tilt_control; + float _roll_weight_mc; + + void fill_mc_att_control_output(); + void fill_fw_att_control_output(); + void set_max_mc(); + void set_max_fw(unsigned pwm_value); + + int parameters_update(); + +}; +#endif diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c new file mode 100644 index 0000000000..76f3ee6c3c --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file tiltrotor_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + */ + +#include + +/** + * Duration of a front transition + * + * Time in seconds used for a transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f); + +/** + * Duration of a back transition + * + * Time in seconds used for a back transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); + +/** + * Position of tilt servo in mc mode + * + * Position of tilt servo in mc mode + * + * @min 0.0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); + +/** + * Position of tilt servo in transition mode + * + * Position of tilt servo in transition mode + * + * @min 0.0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); + +/** + * Position of tilt servo in fw mode + * + * Position of tilt servo in fw mode + * + * @min 0.0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f); + +/** + * Transition airspeed + * + * Airspeed at which we can switch to fw mode + * + * @min 0.0 + * @max 20 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f); + +/** + * Lock elevons in multicopter mode + * + * If set to 1 the elevons are locked in multicopter mode + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0); From 77077cb92ab11b78b4636072c7e1ae9c01c5e0e8 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:57:10 +0200 Subject: [PATCH 074/493] added tailsitter attitude control class --- src/modules/vtol_att_control/tailsitter.cpp | 184 ++++++++++++++++++++ src/modules/vtol_att_control/tailsitter.h | 74 ++++++++ 2 files changed, 258 insertions(+) create mode 100644 src/modules/vtol_att_control/tailsitter.cpp create mode 100644 src/modules/vtol_att_control/tailsitter.h diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp new file mode 100644 index 0000000000..4479783b92 --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + /** + * @file tailsitter.cpp + * + * @author Roman Bapst + * + */ + + #include "tailsitter.h" + #include "vtol_att_control_main.h" + +Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) : +VtolType(att_controller), +_airspeed_tot(0), +_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), +_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) +{ + +} + +Tailsitter::~Tailsitter() +{ + +} + +void Tailsitter::update_vtol_state() +{ + // simply switch between the two modes + if (_manual_control_sp->aux1 < 0.0f) { + _vtol_mode = ROTARY_WING; + } else if (_manual_control_sp->aux1 > 0.0f) { + _vtol_mode = FIXED_WING; + } +} + +void Tailsitter::update_mc_state() +{ + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } +} + +void Tailsitter::process_mc_data() +{ + // scale pitch control with total airspeed + //scale_mc_output(); + fill_mc_att_control_output(); +} + +void Tailsitter::update_fw_state() +{ + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } +} + +void Tailsitter::process_fw_data() +{ + fill_fw_att_control_output(); +} + +void Tailsitter::update_transition_state() +{ + +} + +void Tailsitter::update_external_state() +{ + +} + + void Tailsitter::calc_tot_airspeed() + { + float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama + // calculate momentary power of one engine + float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count; + P = math::constrain(P,1.0f,_params->power_max); + // calculate prop efficiency + float power_factor = 1.0f - P*_params->prop_eff/_params->power_max; + float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; + eta = math::constrain(eta,0.001f,1.0f); // live on the safe side + // calculate induced airspeed by propeller + float v_ind = (airspeed/eta - airspeed)*2.0f; + // calculate total airspeed + float airspeed_raw = airspeed + v_ind; + // apply low-pass filter + _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; +} + +void +Tailsitter::scale_mc_output() +{ + // scale around tuning airspeed + float airspeed; + calc_tot_airspeed(); // estimate air velocity seen by elevons + // if airspeed is not updating, we assume the normal average speed + if (bool nonfinite = !isfinite(_airspeed->true_airspeed_m_s) || + hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { + airspeed = _params->mc_airspeed_trim; + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + } else { + airspeed = _airspeed_tot; + airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max); + } + + _vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed); + _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); +} + +/** +* Prepare message to acutators with data from fw attitude controller. +*/ +void Tailsitter::fill_fw_att_control_output() +{ + /*For the first test in fw mode, only use engines for thrust!!!*/ + _actuators_out_0->control[0] = 0; + _actuators_out_0->control[1] = 0; + _actuators_out_0->control[2] = 0; + _actuators_out_0->control[3] = _actuators_fw_in->control[3]; + /*controls for the elevons */ + _actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon + _actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon + // unused now but still logged + _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle +} + +/** +* Prepare message to acutators with data from mc attitude controller. +*/ +void Tailsitter::fill_mc_att_control_output() +{ + _actuators_out_0->control[0] = _actuators_mc_in->control[0]; + _actuators_out_0->control[1] = _actuators_mc_in->control[1]; + _actuators_out_0->control[2] = _actuators_mc_in->control[2]; + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; + //set neutral position for elevons + _actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon + _actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon +} diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h new file mode 100644 index 0000000000..e681a9bf8b --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + /** + * @file tiltrotor.h + * + * @author Roman Bapst + * + */ + +#ifndef TAILSITTER_H +#define TAILSITTER_H + +#include "vtol_type.h" +#include + +class Tailsitter : public VtolType +{ + +public: + Tailsitter(VtolAttitudeControl * _att_controller); + ~Tailsitter(); + + void update_vtol_state(); + void update_mc_state(); + void process_mc_data(); + void update_fw_state(); + void process_fw_data(); + void update_transition_state(); + void update_external_state(); + +private: + void fill_mc_att_control_output(); + void fill_fw_att_control_output(); + void calc_tot_airspeed(); + void scale_mc_output(); + + float _airspeed_tot; + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + +}; +#endif From 526698854cabd3af875259bf966a9004c65d71df Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:57:54 +0200 Subject: [PATCH 075/493] adapt vtol attitude control class to new vtol type classes --- .../vtol_att_control_main.cpp | 437 +++--------------- .../vtol_att_control/vtol_att_control_main.h | 212 +++++++++ .../vtol_att_control_params.c | 8 + 3 files changed, 293 insertions(+), 364 deletions(-) create mode 100644 src/modules/vtol_att_control/vtol_att_control_main.h diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index fdb4de4343..a565c618e3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -43,166 +43,7 @@ * @author Thomas Gubler * */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drivers/drv_pwm_output.h" -#include - -#include - - -extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); - -class VtolAttitudeControl -{ -public: - - VtolAttitudeControl(); - ~VtolAttitudeControl(); - - int start(); /* start the task and return OK on success */ - - -private: -//******************flags & handlers****************************************************** - bool _task_should_exit; - int _control_task; //task handle for VTOL attitude controller - - /* handlers for subscriptions */ - int _v_att_sub; //vehicle attitude subscription - int _v_att_sp_sub; //vehicle attitude setpoint subscription - int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _v_control_mode_sub; //vehicle control mode subscription - int _params_sub; //parameter updates subscription - int _manual_control_sp_sub; //manual control setpoint subscription - int _armed_sub; //arming status subscription - int _local_pos_sub; // sensor subscription - int _airspeed_sub; // airspeed subscription - int _battery_status_sub; // battery status subscription - - int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs - int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs - - //handlers for publishers - orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) - orb_advert_t _actuators_1_pub; - orb_advert_t _vtol_vehicle_status_pub; - orb_advert_t _v_rates_sp_pub; -//*******************data containers*********************************************************** - struct vehicle_attitude_s _v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s _v_control_mode; //vehicle control mode - struct vtol_vehicle_status_s _vtol_vehicle_status; - struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s _armed; //actuator arming status - struct vehicle_local_position_s _local_pos; - struct airspeed_s _airspeed; // airspeed - struct battery_status_s _batt_status; // battery status - - struct { - param_t idle_pwm_mc; //pwm value for idle in mc mode - param_t vtol_motor_count; - param_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode - float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) - float mc_airspeed_trim; // trim airspeed in multicopter mode - float mc_airspeed_max; // max airpseed in multicopter mode - float fw_pitch_trim; // trim for neutral elevon position in fw mode - float power_max; // maximum power of one engine - float prop_eff; // factor to calculate prop efficiency - float arsp_lp_gain; // total airspeed estimate low pass gain - } _params; - - struct { - param_t idle_pwm_mc; - param_t vtol_motor_count; - param_t vtol_fw_permanent_stab; - param_t mc_airspeed_min; - param_t mc_airspeed_trim; - param_t mc_airspeed_max; - param_t fw_pitch_trim; - param_t power_max; - param_t prop_eff; - param_t arsp_lp_gain; - } _params_handles; - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - - /* for multicopters it is usual to have a non-zero idle speed of the engines - * for fixed wings we want to have an idle speed of zero since we do not want - * to waste energy when gliding. */ - bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" - unsigned _motor_count; // number of motors - float _airspeed_tot; - float _tilt_control; -//*****************Member functions*********************************************************************** - - void task_main(); //main task - static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. - - void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - void vehicle_manual_poll(); //Check for changes in manual inputs. - void arming_status_poll(); //Check for arming status updates. - void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - void vehicle_rates_sp_mc_poll(); - void vehicle_rates_sp_fw_poll(); - void vehicle_local_pos_poll(); // Check for changes in sensor values - void vehicle_airspeed_poll(); // Check for changes in airspeed - void vehicle_battery_poll(); // Check for battery updates - void parameters_update_poll(); //Check if parameters have changed - int parameters_update(); //Update local paraemter cache - void fill_mc_att_control_output(); //write mc_att_control results to actuator message - void fill_fw_att_control_output(); //write fw_att_control results to actuator message - void fill_mc_att_rates_sp(); - void fill_fw_att_rates_sp(); - void set_idle_fw(); - void set_idle_mc(); - void scale_mc_output(); - void calc_tot_airspeed(); // estimated airspeed seen by elevons -}; +#include "vtol_att_control_main.h" namespace VTOL_att_control { @@ -230,19 +71,12 @@ VtolAttitudeControl::VtolAttitudeControl() : _battery_status_sub(-1), //init publication handlers - _actuators_0_pub(-1), - _actuators_1_pub(-1), - _vtol_vehicle_status_pub(-1), - _v_rates_sp_pub(-1), + _actuators_0_pub(0), + _actuators_1_pub(0), + _vtol_vehicle_status_pub(0), + _v_rates_sp_pub(0) - _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input")) { - - flag_idle_mc = true; - _airspeed_tot = 0.0f; - _tilt_control = 0.0f; - memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); @@ -276,9 +110,20 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.power_max = param_find("VT_POWER_MAX"); _params_handles.prop_eff = param_find("VT_PROP_EFF"); _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); + _params_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ parameters_update(); + + if (_params.vtol_type == 0) { + _tailsitter = new Tailsitter(this); + _vtol_type = _tailsitter; + } else if (_params.vtol_type == 1) { + _tiltrotor = new Tiltrotor(this); + _vtol_type = _tiltrotor; + } else { + _task_should_exit = true; + } } /** @@ -470,6 +315,7 @@ int VtolAttitudeControl::parameters_update() { float v; + int l; /* idle pwm for mc mode */ param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc); @@ -507,42 +353,12 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.arsp_lp_gain, &v); _params.arsp_lp_gain = v; + param_get(_params_handles.vtol_type, &l); + _params.vtol_type = l; + return OK; } -/** -* Prepare message to acutators with data from mc attitude controller. -*/ -void VtolAttitudeControl::fill_mc_att_control_output() -{ - _actuators_out_0.control[0] = _actuators_mc_in.control[0]; - _actuators_out_0.control[1] = _actuators_mc_in.control[1]; - _actuators_out_0.control[2] = _actuators_mc_in.control[2]; - _actuators_out_0.control[3] = _actuators_mc_in.control[3]; - //set neutral position for elevons - _actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon - _actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon - _actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control -} - -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void VtolAttitudeControl::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0.control[0] = 0; - _actuators_out_0.control[1] = 0; - _actuators_out_0.control[2] = 0; - _actuators_out_0.control[3] = _actuators_fw_in.control[3]; - /*controls for the elevons */ - _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon - _actuators_out_1.control[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim; // pitch elevon - // unused now but still logged - _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw - _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle -} - /** * Prepare message for mc attitude rates setpoint topic */ @@ -565,109 +381,6 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } -/** -* Adjust idle speed for fw mode. -*/ -void VtolAttitudeControl::set_idle_fw() -{ - int ret; - char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - unsigned pwm_value = PWM_LOWEST_MIN; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < _params.vtol_motor_count; i++) { - - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -/** -* Adjust idle speed for mc mode. -*/ -void VtolAttitudeControl::set_idle_mc() -{ - int ret; - unsigned servo_count; - char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - unsigned pwm_value = _params.idle_pwm_mc; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < _params.vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -void -VtolAttitudeControl::scale_mc_output() { - // scale around tuning airspeed - float airspeed; - calc_tot_airspeed(); // estimate air velocity seen by elevons - // if airspeed is not updating, we assume the normal average speed - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { - airspeed = _params.mc_airspeed_trim; - if (nonfinite) { - perf_count(_nonfinite_input_perf); - } - } else { - airspeed = _airspeed_tot; - airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max); - } - - _vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging - /* - * For scaling our actuators using anything less than the min (close to stall) - * speed doesn't make any sense - its the strongest reasonable deflection we - * want to do in flight and its the baseline a human pilot would choose. - * - * Forcing the scaling to this value allows reasonable handheld tests. - */ - float airspeed_scaling = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed); - _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); -} - -void VtolAttitudeControl::calc_tot_airspeed() { - float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama - // calculate momentary power of one engine - float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count; - P = math::constrain(P,1.0f,_params.power_max); - // calculate prop efficiency - float power_factor = 1.0f - P*_params.prop_eff/_params.power_max; - float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; - eta = math::constrain(eta,0.001f,1.0f); // live on the safe side - // calculate induced airspeed by propeller - float v_ind = (airspeed/eta - airspeed)*2.0f; - // calculate total airspeed - float airspeed_raw = airspeed + v_ind; - // apply low-pass filter - _airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; -} - void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -701,8 +414,7 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; // make sure we start with idle in mc mode - set_idle_mc(); - flag_idle_mc = true; + _vtol_type->set_idle_mc(); /* wakeup source*/ struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ @@ -764,83 +476,80 @@ void VtolAttitudeControl::task_main() vehicle_airspeed_poll(); vehicle_battery_poll(); + // update the vtol state machine which decides which mode we are in + _vtol_type->update_vtol_state(); - if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */ + // check in which mode we are in and call mode specific functions + if (_vtol_type->get_mode() == ROTARY_WING) { + // vehicle is in rotary wing mode _vtol_vehicle_status.vtol_in_rw_mode = true; - if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ - set_idle_mc(); - flag_idle_mc = true; - } + _vtol_type->update_mc_state(); - /* got data from mc_att_controller */ + // got data from mc attitude controller if (fds[0].revents & POLLIN) { - vehicle_manual_poll(); /* update remote input */ orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - // scale pitch control with total airspeed - scale_mc_output(); + _vtol_type->process_mc_data(); - fill_mc_att_control_output(); fill_mc_att_rates_sp(); - - /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled) - { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } - } } - } - - if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ + } else if (_vtol_type->get_mode() == FIXED_WING) { + // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; - if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ - set_idle_fw(); - flag_idle_mc = false; + _vtol_type->update_fw_state(); + + // got data from fw attitude controller + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + vehicle_manual_poll(); + + _vtol_type->process_fw_data(); + + fill_fw_att_rates_sp(); + } + } else if (_vtol_type->get_mode() == TRANSITION) { + // vehicle is doing a transition + bool got_new_data = false; + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + got_new_data = true; } - if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ + if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); - vehicle_manual_poll(); //update remote input + got_new_data = true; + } - fill_fw_att_control_output(); - fill_fw_att_rates_sp(); + // update transition state if got any new data + if (got_new_data) { + _vtol_type->update_transition_state(); + } - /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) - { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + } else if (_vtol_type->get_mode() == EXTERNAL) { + // we are using external module to generate attitude/thrust setpoint + _vtol_type->update_external_state(); + } - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) + { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); } } - } // publish the attitude rates setpoint if(_v_rates_sp_pub > 0) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h new file mode 100644 index 0000000000..2772f9bcb1 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VTOL_att_control_main.cpp + * Implementation of an attitude controller for VTOL airframes. This module receives data + * from both the fixed wing- and the multicopter attitude controllers and processes it. + * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- + * flight or transition). It also publishes the resulting controls on the actuator controls topics. + * + * @author Roman Bapst + * @author Lorenz Meier + * @author Thomas Gubler + * + */ +#ifndef VTOL_ATT_CONTROL_MAIN_H +#define VTOL_ATT_CONTROL_MAIN_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tiltrotor.h" +#include "tailsitter.h" + + +extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); + + +class VtolAttitudeControl +{ +public: + + VtolAttitudeControl(); + ~VtolAttitudeControl(); + + int start(); /* start the task and return OK on success */ + + struct vehicle_attitude_s* get_att () {return &_v_att;} + struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;} + struct vehicle_rates_setpoint_s* get_rates_sp () {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s* get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s* get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s* get_manual_control_sp () {return &_manual_control_sp;} + struct vehicle_control_mode_s* get_control_mode () {return &_v_control_mode;} + struct vtol_vehicle_status_s* get_vehicle_status () {return &_vtol_vehicle_status;} + struct actuator_controls_s* get_actuators_out0 () {return &_actuators_out_0;} + struct actuator_controls_s* get_actuators_out1 () {return &_actuators_out_1;} + struct actuator_controls_s* get_actuators_mc_in () {return &_actuators_mc_in;} + struct actuator_controls_s* get_actuators_fw_in () {return &_actuators_fw_in;} + struct actuator_armed_s* get_armed () {return &_armed;} + struct vehicle_local_position_s* get_local_pos () {return &_local_pos;} + struct airspeed_s* get_airspeed () {return &_airspeed;} + struct battery_status_s* get_batt_status () {return &_batt_status;} + + struct Params* get_params () {return &_params;} + + +private: +//******************flags & handlers****************************************************** + bool _task_should_exit; + int _control_task; //task handle for VTOL attitude controller + + /* handlers for subscriptions */ + int _v_att_sub; //vehicle attitude subscription + int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription + int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription + int _v_control_mode_sub; //vehicle control mode subscription + int _params_sub; //parameter updates subscription + int _manual_control_sp_sub; //manual control setpoint subscription + int _armed_sub; //arming status subscription + int _local_pos_sub; // sensor subscription + int _airspeed_sub; // airspeed subscription + int _battery_status_sub; // battery status subscription + + int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs + int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs + + //handlers for publishers + orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) + orb_advert_t _actuators_1_pub; + orb_advert_t _vtol_vehicle_status_pub; + orb_advert_t _v_rates_sp_pub; +//*******************data containers*********************************************************** + struct vehicle_attitude_s _v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s _v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s _vtol_vehicle_status; + struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s _armed; //actuator arming status + struct vehicle_local_position_s _local_pos; + struct airspeed_s _airspeed; // airspeed + struct battery_status_s _batt_status; // battery status + + Params _params; // struct holding the parameters + + struct { + param_t idle_pwm_mc; + param_t vtol_motor_count; + param_t vtol_fw_permanent_stab; + param_t mc_airspeed_min; + param_t mc_airspeed_trim; + param_t mc_airspeed_max; + param_t fw_pitch_trim; + param_t power_max; + param_t prop_eff; + param_t arsp_lp_gain; + param_t vtol_type; + } _params_handles; + + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ + unsigned _motor_count; // number of motors + float _airspeed_tot; + + VtolType * _vtol_type; // base class for different vtol types + Tiltrotor * _tiltrotor; // tailsitter vtol type + Tailsitter * _tailsitter; // tiltrotor vtol type + +//*****************Member functions*********************************************************************** + + void task_main(); //main task + static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. + + void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. + void vehicle_manual_poll(); //Check for changes in manual inputs. + void arming_status_poll(); //Check for arming status updates. + void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output + void vehicle_rates_sp_mc_poll(); + void vehicle_rates_sp_fw_poll(); + void vehicle_local_pos_poll(); // Check for changes in sensor values + void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_battery_poll(); // Check for battery updates + void parameters_update_poll(); //Check if parameters have changed + int parameters_update(); //Update local paraemter cache + void fill_mc_att_rates_sp(); + void fill_fw_att_rates_sp(); +}; + +#endif 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 6da28b1304..429d44c46c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -142,3 +142,11 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); */ PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); +/** + * VTOL Type (Tailsitter=0, Tiltrotor=1) + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_TYPE, 0); From 12feef85bfd9ae92eaa50c1efcc2d27d2c7bff72 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:58:47 +0200 Subject: [PATCH 076/493] lower lowest allowed max pwm value to be able to cut rear motors for firefly6 in fw mode --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6271ad2086..2fb9469c8a 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -93,7 +93,7 @@ __BEGIN_DECLS /** * Lowest PWM allowed as the maximum PWM */ -#define PWM_LOWEST_MAX 1400 +#define PWM_LOWEST_MAX 950 /** * Do not output a channel with this value From b3c3d6634c31322df1e17666dc97e44bcbe47302 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 00:00:23 +0200 Subject: [PATCH 077/493] added vtol types --- src/modules/vtol_att_control/module.mk | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index 0cf3072c8f..ad6efd2b27 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -38,7 +38,10 @@ MODULE_COMMAND = vtol_att_control SRCS = vtol_att_control_main.cpp \ - vtol_att_control_params.c + vtol_att_control_params.c \ + tiltrotor_params.c \ + tiltrotor.cpp \ + vtol_type.cpp \ + tailsitter.cpp EXTRACXXFLAGS = -Wno-write-strings - From d320dc8ada0ca85722a1b57d5550fd5333d53980 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:45:41 +0200 Subject: [PATCH 078/493] added VTOL type param to VTOL configuration files --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 3 ++- ROMFS/px4fmu_common/init.d/13002_firefly6 | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 5c00041492..912202f962 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -1,7 +1,7 @@ # # Generic configuration file for caipirinha VTOL version # -# Roman Bapst +# Roman Bapst # sh /etc/init.d/rc.vtol_defaults @@ -13,3 +13,4 @@ set PWM_MAX 2000 set PWM_RATE 400 param set VT_MOT_COUNT 2 param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index ed90dabf41..963341d138 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -2,7 +2,7 @@ # # Generic configuration file for BirdsEyeView Aerobotics FireFly6 # -# Roman Bapst +# Roman Bapst # sh /etc/init.d/rc.vtol_defaults @@ -41,3 +41,4 @@ set MAV_TYPE 21 param set VT_MOT_COUNT 6 param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 1 From 4e9fd5b2a4be4ef4054781f1ed5e6f6896e2fca8 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:46:20 +0200 Subject: [PATCH 079/493] rotate attitude for fw mode only if VTOL is a tailsitter --- .../fw_att_control/fw_att_control_main.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index fe27de14f5..ccf12a0791 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -193,12 +193,14 @@ private: float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ - float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ - float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ - float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ - float man_pitch_max; /**< Max Pitch in rad */ + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ + + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ } _parameters; /**< local copies of interesting parameters */ @@ -241,6 +243,8 @@ private: param_t man_roll_max; param_t man_pitch_max; + param_t vtol_type; + } _parameter_handles; /**< handles for interesting parameters */ @@ -404,6 +408,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); + /* fetch initial parameter values */ parameters_update(); } @@ -481,6 +487,8 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -703,10 +711,8 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - if (_vehicle_status.is_vtol) { - /* vehicle type is VTOL, need to modify attitude! - * The following modification to the attitude is vehicle specific and in this case applies - * to tail-sitter models !!! + if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { + /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to * modify the estimated attitude for the fixed wing operation. From 51b1968bddc4d976b047f5edfe165dbf4259de4f Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:51:02 +0200 Subject: [PATCH 080/493] added configuration file for tailsitter with motors in quad x configuration --- .../px4fmu_common/init.d/13003_quad_tailsitter | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13003_quad_tailsitter diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter new file mode 100644 index 0000000000..daad3d9351 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter @@ -0,0 +1,17 @@ +# +# Generic configuration file for a tailsitter with motors in X configuration. +# +# Roman Bapst +# + +sh /etc/init.d/rc.vtol_defaults + +set MIXER quad_x_vtol + +set PWM_OUT 1234 +set PWM_MAX 2000 +set PWM_RATE 400 +set MAV_TYPE 20 +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 0 From fb70b0a2b59bfbf517ddb6bd0a5f989bfed16666 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:51:48 +0200 Subject: [PATCH 081/493] added mixer file for tailsitter with motors in quad x configuration and 2 elevons --- .../px4fmu_common/mixers/quad_x_vtol.main.mix | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix new file mode 100644 index 0000000000..4fd323353a --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix @@ -0,0 +1,18 @@ +Mixer for Tailsitter with x motor configuration and elevons +=========================================================== + +This file defines a single mixer for tailsitter with motors in X configuration. All controls +are mixed 100%. + +R: 4x 10000 10000 10000 0 + +#mixer for the elevons +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 1 -10000 -10000 0 -10000 10000 From aade901ef02f604c468359d8c9e937c717035f9c Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 09:02:55 +0200 Subject: [PATCH 082/493] added new tailsitter type to autostart list --- ROMFS/px4fmu_common/init.d/rc.autostart | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index de81795b44..a45ceeb276 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -268,6 +268,14 @@ then sh /etc/init.d/13002_firefly6 fi +# +# Tailsitter with 4 motors in x config +# +if param compare SYS_AUTOSTART 13003 +then + sh /etc/init.d/13003_quad_tailsitter +fi + # # TriCopter Y Yaw+ # From 60857c7940d13af7d0168e7950a9799d30c3bb68 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 09:46:40 +0200 Subject: [PATCH 083/493] add option to lock elevons for tailsitters in mc mode --- src/modules/vtol_att_control/tailsitter.cpp | 11 ++++++++--- src/modules/vtol_att_control/tiltrotor_params.c | 11 ----------- .../vtol_att_control/vtol_att_control_main.cpp | 5 +++++ src/modules/vtol_att_control/vtol_att_control_main.h | 1 + .../vtol_att_control/vtol_att_control_params.c | 11 +++++++++++ src/modules/vtol_att_control/vtol_type.h | 1 + 6 files changed, 26 insertions(+), 14 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 4479783b92..358f636561 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -178,7 +178,12 @@ void Tailsitter::fill_mc_att_control_output() _actuators_out_0->control[1] = _actuators_mc_in->control[1]; _actuators_out_0->control[2] = _actuators_mc_in->control[2]; _actuators_out_0->control[3] = _actuators_mc_in->control[3]; - //set neutral position for elevons - _actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon - _actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon + + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + } else { + _actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon + _actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon + } } diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 76f3ee6c3c..7d233f6f50 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -105,14 +105,3 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f); - -/** - * Lock elevons in multicopter mode - * - * If set to 1 the elevons are locked in multicopter mode - * - * @min 0 - * @max 1 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index a565c618e3..b70cd19dd8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -111,6 +111,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.prop_eff = param_find("VT_PROP_EFF"); _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); _params_handles.vtol_type = param_find("VT_TYPE"); + _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); /* fetch initial parameter values */ parameters_update(); @@ -356,6 +357,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.vtol_type, &l); _params.vtol_type = l; + /* vtol lock elevons in multicopter */ + param_get(_params_handles.elevons_mc_lock, &l); + _params.elevons_mc_lock = l; + return OK; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2772f9bcb1..43e8969929 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -176,6 +176,7 @@ private: param_t prop_eff; param_t arsp_lp_gain; param_t vtol_type; + param_t elevons_mc_lock; } _params_handles; /* for multicopters it is usual to have a non-zero idle speed of the engines 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 429d44c46c..f302314a23 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -150,3 +150,14 @@ PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_TYPE, 0); + +/** + * Lock elevons in multicopter mode + * + * If set to 1 the elevons are locked in multicopter mode + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 57448a7587..bbe6a8642e 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -53,6 +53,7 @@ struct Params { float prop_eff; // factor to calculate prop efficiency float arsp_lp_gain; // total airspeed estimate low pass gain int vtol_type; + int elevons_mc_lock; // lock elevons in multicopter mode }; enum mode { From fc7dc297fc168465456b868cfd5cef26174f76d6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 09:48:07 +0200 Subject: [PATCH 084/493] lock elevons in mc mode for tailsitter with motors in x config --- ROMFS/px4fmu_common/init.d/13003_quad_tailsitter | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter index daad3d9351..17560526a3 100644 --- a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter +++ b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter @@ -15,3 +15,4 @@ set MAV_TYPE 20 param set VT_MOT_COUNT 4 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 0 +param set VT_ELEV_MC_LOCK 1 From c4d92ff05bce61ea8418acdd40ccf133d199033e Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 14:28:27 +0200 Subject: [PATCH 085/493] enable receiving data over network port --- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 55 ++++++++++++++++-------- 2 files changed, 39 insertions(+), 18 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a5468a86cc..f05b759d1c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -45,9 +45,9 @@ #ifdef __PX4_NUTTX #include #else -#include #include #include +#include #endif #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 90decb9e47..8c4b16b49c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1624,34 +1624,56 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) void * MavlinkReceiver::receive_thread(void *arg) { -#ifndef __PX4_POSIX - int uart_fd = _mavlink->get_uart_fd(); const int timeout = 500; uint8_t buf[32]; - mavlink_message_t msg; - /* set thread name */ - char thread_name[24]; - sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); - prctl(PR_SET_NAME, thread_name, getpid()); - struct pollfd fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; + int uart_fd = -1; + + if (_mavlink->get_protocol() == SERIAL) { + uart_fd = _mavlink->get_uart_fd(); +#ifndef __PX4_POSIX + /* set thread name */ + char thread_name[24]; + sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); + prctl(PR_SET_NAME, thread_name, getpid()); +#endif + + fds[0].fd = uart_fd; + fds[0].events = POLLIN; + } +#ifdef __PX4_POSIX + struct sockaddr_in srcaddr; + socklen_t addrlen = sizeof(srcaddr); + if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) { + fds[0].fd = _mavlink->get_socket_fd(); + fds[0].events = POLLIN; + } +#endif ssize_t nread = 0; while (!_mavlink->_task_should_exit) { - if (poll(fds, 1, timeout) > 0) { - - /* non-blocking read. read may return negative values */ - if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { - /* to avoid reading very small chunks wait for data before reading */ - usleep(1000); + if (poll(&fds[0], 1, timeout) > 0) { + if (_mavlink->get_protocol() == SERIAL) { + /* non-blocking read. read may return negative values */ + if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } } +#ifdef __PX4_POSIX + if (_mavlink->get_protocol() == UDP) { + if (fds[0].revents & POLLIN) { + nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); + } + } else { + // could be TCP or other protocol + } +#endif /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { @@ -1667,7 +1689,6 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->count_rxbytes(nread); } } -#endif return NULL; } From ecbc28646914cb23c25ced1d6c69c55587798657 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 14:27:42 +0200 Subject: [PATCH 086/493] added function to return socket fd --- src/modules/mavlink/mavlink_main.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f05b759d1c..1b6904655a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -325,6 +325,8 @@ public: unsigned short get_network_port() { return _network_port; } + int get_socket_fd () { return _socket_fd; }; + protected: Mavlink *next; From 655617f958ff9f8af3544f9dc1661574abedd5f9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 15:54:36 +0200 Subject: [PATCH 087/493] mavlink: - implement get_free_tx_buf() for UDP and TCP - gefine get_uart_fd for all platforms --- src/modules/mavlink/mavlink_main.cpp | 7 +++++-- src/modules/mavlink/mavlink_main.h | 2 -- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c53eccdb2a..619e076dfe 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -435,7 +435,6 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) } } -#ifndef __PX4_POSIX int Mavlink::get_uart_fd(unsigned index) { @@ -453,7 +452,6 @@ Mavlink::get_uart_fd() { return _uart_fd; } -#endif // __PX4_POSIX int Mavlink::get_instance_id() @@ -810,6 +808,11 @@ Mavlink::get_free_tx_buf() #endif + // if we are using network sockets, return max lenght of one packet + if (get_protocol() == UDP || get_protocol() == TCP ) { + return 1500; + } + return buf_free; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1b6904655a..4a42f0bce1 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,11 +123,9 @@ public: static void forward_message(const mavlink_message_t *msg, Mavlink *self); -#ifndef __PX4_QURT static int get_uart_fd(unsigned index); int get_uart_fd(); -#endif /** * Get the MAVLink system id. From bf24d42a79844ff1b7cc4c2186c4817e81713b32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 10:44:36 -0700 Subject: [PATCH 088/493] POSIX: Fix SITL startup script --- posix-configs/SITL/init/rcS | 38 +++++++++++++------------------------ 1 file changed, 13 insertions(+), 25 deletions(-) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index d2057d4b03..c89714c02c 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -1,5 +1,8 @@ uorb start param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.05 +param set MC_ROLLRATE_P 0.05 dataman start mavlink start -u 14556 -r 60000 simulator start -s @@ -7,6 +10,15 @@ param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1310720 param set CAL_ACC1_ID 1376256 param set CAL_MAG0_ID 196608 +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 rgbled start tone_alarm start gyrosim start @@ -14,34 +26,10 @@ accelsim start barosim start adcsim start gpssim start +hil mode_pwm commander start sensors start ekf_att_pos_estimator start mc_pos_control start mc_att_control start -hil mode_pwm -param set MAV_TYPE 2 -param set RC1_MAX 2015 -param set RC1_MIN 996 -param set RC1_TRIM 1502 -param set RC1_REV -1 -param set RC2_MAX 2016 -param set RC2_MIN 995 -param set RC2_TRIM 1500 -param set RC3_MAX 2003 -param set RC3_MIN 992 -param set RC3_TRIM 992 -param set RC4_MAX 2011 -param set RC4_MIN 997 -param set RC4_TRIM 1504 -param set RC4_REV -1 -param set RC6_MAX 2016 -param set RC6_MIN 992 -param set RC6_TRIM 1504 -param set RC_CHAN_CNT 8 -param set RC_MAP_MODE_SW 5 -param set RC_MAP_POSCTL_SW 7 -param set RC_MAP_RETURN_SW 8 -param set MC_PITCHRATE_P 0.05 -param set MC_ROLLRATE_P 0.05 mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix From 79d9e1be8d521949749e588d7a0ba9abcf56f522 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 10:44:58 -0700 Subject: [PATCH 089/493] sensors app: Load missing param --- 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 0be31046b1..a2d651e337 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -638,6 +638,7 @@ Sensors::Sensors() : (void)param_find("CAL_MAG2_ROT"); (void)param_find("SYS_PARAM_VER"); (void)param_find("SYS_AUTOSTART"); + (void)param_find("SYS_AUTOCONFIG"); /* fetch initial parameter values */ parameters_update(); From 878284701d760890d701f19af047bae1fc355abe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 10:45:26 -0700 Subject: [PATCH 090/493] POSIX: Simulator: Use port 14560, since 14550 is QGroundControls default port --- src/modules/simulator/simulator_mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index e4403fd221..0c411edca5 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -40,7 +40,7 @@ using namespace simulator; #define SEND_INTERVAL 20 -#define UDP_PORT 14550 +#define UDP_PORT 14560 #define PIXHAWK_DEVICE "/dev/ttyACM0" #define PRESS_GROUND 101325.0f From f7fe6a037d7cddcc1d956df63b9cce41c279d326 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 19 Jun 2015 11:39:08 -0700 Subject: [PATCH 091/493] Converted getopt use to px4_getopt In the posix and qurt builds, getopt is not thread safe so px4_getopt should be used instead. Signed-off-by: Mark Charlebois --- makefiles/posix/toolchain_native.mk | 2 +- src/modules/sdlog2/sdlog2.c | 9 +- .../posix/px4_layer/px4_posix_tasks.cpp | 1 + src/systemcmds/esc_calib/esc_calib.c | 120 +++++++++++------- src/systemcmds/reboot/reboot.c | 16 +-- 5 files changed, 86 insertions(+), 62 deletions(-) diff --git a/makefiles/posix/toolchain_native.mk b/makefiles/posix/toolchain_native.mk index ff4cdc4702..f58e55314b 100644 --- a/makefiles/posix/toolchain_native.mk +++ b/makefiles/posix/toolchain_native.mk @@ -120,7 +120,7 @@ ifeq ($(CONFIG_BOARD),) $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ - -Dnoreturn_function= \ + -Dnoreturn_function=__attribute__\(\(noreturn\)\) \ -I$(PX4_BASE)/src/modules/systemlib \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/posix/include \ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 270804075e..693ff0d034 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -44,6 +44,7 @@ #include #include +#include #include #include #include @@ -895,10 +896,12 @@ int sdlog2_thread_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "r:b:eatx")) != EOF) { + int myoptind = 1; + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "r:b:eatx", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'r': { - unsigned long r = strtoul(optarg, NULL, 10); + unsigned long r = strtoul(myoptarg, NULL, 10); if (r == 0) { r = 1; @@ -909,7 +912,7 @@ int sdlog2_thread_main(int argc, char *argv[]) break; case 'b': { - unsigned long s = strtoul(optarg, NULL, 10); + unsigned long s = strtoul(myoptarg, NULL, 10); if (s < 1) { s = 1; diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index c65fadcc1a..ac87ddfd55 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -98,6 +98,7 @@ void px4_systemreset(bool to_bootloader) { PX4_WARN("Called px4_system_reset"); + exit(0); } px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index f04ca29ff9..8b432f1638 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -39,7 +39,9 @@ */ #include -#include +#include +#include +#include #include #include @@ -52,15 +54,9 @@ #include #include -#include -#include -#include -#include - #include #include "systemlib/systemlib.h" -#include "systemlib/err.h" #include "drivers/drv_pwm_output.h" #include @@ -76,9 +72,9 @@ static void usage(const char *reason) { if (reason != NULL) - warnx("%s", reason); + PX4_ERR("%s", reason); - errx(1, + PX4_ERR( "usage:\n" "esc_calib\n" " [-d PWM output device (defaults to " PWM_OUTPUT0_DEVICE_PATH ")\n" @@ -93,7 +89,7 @@ usage(const char *reason) int esc_calib_main(int argc, char *argv[]) { - char *dev = PWM_OUTPUT0_DEVICE_PATH; + const char *dev = PWM_OUTPUT0_DEVICE_PATH; char *ep; int ch; int ret; @@ -114,21 +110,24 @@ esc_calib_main(int argc, char *argv[]) if (argc < 2) { usage("no channels provided"); + return 1; } int arg_consumed = 0; - while ((ch = getopt(argc, argv, "d:c:m:al:h:")) != EOF) { + int myoptind = 1; + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "d:c:m:al:h:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'd': - dev = optarg; + dev = myoptarg; arg_consumed += 2; break; case 'c': /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ - channels = strtoul(optarg, &ep, 0); + channels = strtoul(myoptarg, &ep, 0); while ((single_ch = channels % 10)) { @@ -139,9 +138,11 @@ esc_calib_main(int argc, char *argv[]) case 'm': /* Read in mask directly */ - set_mask = strtoul(optarg, &ep, 0); - if (*ep != '\0') + set_mask = strtoul(myoptarg, &ep, 0); + if (*ep != '\0') { usage("bad set_mask value"); + return 1; + } break; case 'a': @@ -153,27 +154,34 @@ esc_calib_main(int argc, char *argv[]) case 'l': /* Read in custom low value */ - pwm_low = strtoul(optarg, &ep, 0); - if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) + pwm_low = strtoul(myoptarg, &ep, 0); + if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) { usage("low PWM invalid"); + return 1; + } break; case 'h': /* Read in custom high value */ - pwm_high = strtoul(optarg, &ep, 0); - if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) + pwm_high = strtoul(myoptarg, &ep, 0); + if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) { usage("high PWM invalid"); + return 1; + } break; default: usage(NULL); + return 1; } } if (set_mask == 0) { usage("no channels chosen"); + return 1; } if (pwm_low > pwm_high) { usage("low pwm is higher than high pwm"); + return 1; } /* make sure no other source is publishing control values now */ @@ -191,9 +199,10 @@ esc_calib_main(int argc, char *argv[]) orb_check(act_sub, &orb_updated); if (orb_updated) { - errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" + PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" "\tmc_att_control stop\n" "\tfw_att_control stop\n"); + return 1; } printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" @@ -211,24 +220,22 @@ esc_calib_main(int argc, char *argv[]) ret = poll(&fds, 1, 0); if (ret > 0) { - read(0, &c, 1); if (c == 'y' || c == 'Y') { - break; } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); - exit(0); + return 0; } else if (c == 'n' || c == 'N') { printf("ESC calibration aborted\n"); - exit(0); + return 0; } else { printf("Unknown input, ESC calibration aborted\n"); - exit(0); + return 0; } } @@ -239,24 +246,32 @@ esc_calib_main(int argc, char *argv[]) /* open for ioctl only */ int fd = open(dev, 0); - if (fd < 0) - err(1, "can't open %s", dev); + if (fd < 0) { + PX4_ERR("can't open %s", dev); + return 1; + } /* get number of channels available on the device */ ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels); - if (ret != OK) - err(1, "PWM_SERVO_GET_COUNT"); + if (ret != OK) { + PX4_ERR("PWM_SERVO_GET_COUNT"); + return 1; + } /* tell IO/FMU that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (ret != OK) - err(1, "PWM_SERVO_SET_ARM_OK"); + if (ret != OK) { + PX4_ERR("PWM_SERVO_SET_ARM_OK"); + return 1; + } /* tell IO/FMU that the system is armed (it will output values if safety is off) */ ret = ioctl(fd, PWM_SERVO_ARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_ARM"); + if (ret != OK) { + PX4_ERR("PWM_SERVO_ARM"); + return 1; + } - warnx("Outputs armed"); + printf("Outputs armed"); /* wait for user confirmation */ @@ -273,24 +288,24 @@ esc_calib_main(int argc, char *argv[]) if (set_mask & 1< 0) { - read(0, &c, 1); if (c == 13) { - break; } else if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("ESC calibration exited"); - exit(0); + printf("ESC calibration exited"); + goto done; } } @@ -310,24 +325,24 @@ esc_calib_main(int argc, char *argv[]) if (set_mask & 1< 0) { - read(0, &c, 1); if (c == 13) { - break; } else if (c == 0x03 || c == 0x63 || c == 'q') { printf("ESC calibration exited\n"); - exit(0); + goto done; } } @@ -337,12 +352,19 @@ esc_calib_main(int argc, char *argv[]) /* disarm */ ret = ioctl(fd, PWM_SERVO_DISARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_DISARM"); + if (ret != OK) { + PX4_ERR("PWM_SERVO_DISARM"); + goto cleanup; + } - warnx("Outputs disarmed"); + printf("Outputs disarmed"); printf("ESC calibration finished\n"); - exit(0); +done: + close(fd); + return 0; +cleanup: + close(fd); + return 1; } diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index de4d77d5fd..8de6b09985 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -38,12 +38,10 @@ */ #include -#include -#include -#include - +#include +#include +#include #include -#include __EXPORT int reboot_main(int argc, char *argv[]); @@ -52,14 +50,16 @@ int reboot_main(int argc, char *argv[]) int ch; bool to_bootloader = false; - while ((ch = getopt(argc, argv, "b")) != -1) { + int myoptind = 1; + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) { switch (ch) { case 'b': to_bootloader = true; break; default: - errx(1, "usage: reboot [-b]\n" + PX4_ERR("usage: reboot [-b]\n" " -b reboot into the bootloader"); } @@ -67,5 +67,3 @@ int reboot_main(int argc, char *argv[]) px4_systemreset(to_bootloader); } - - From d81d20ff0e3a8a5592c57951be1a2b2c7cb7bc29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 11:51:37 -0700 Subject: [PATCH 092/493] VDev: Add missing break --- src/drivers/device/vdev.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 89a3da3e0a..d992851309 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -324,6 +324,7 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg) case DEVIOCGDEVICEID: ret = (int)_device_id.devid; PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret); + break; default: break; } From 27cec4a977cdf19c02f8ad9e03d0eaa8aa4149a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 11:52:07 -0700 Subject: [PATCH 093/493] VDev POSIX: Fix non-POSIX conformant return value handling --- src/drivers/device/vdev_posix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 727b92a1ed..975700d4e8 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -201,7 +201,7 @@ int px4_ioctl(int fd, int cmd, unsigned long arg) px4_errno = -ret; } - return (ret == 0) ? PX4_OK : PX4_ERROR; + return ret; } int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) From 3627456dd6c8668927ab9838b96f4dab3e5d1892 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 12:01:48 -0700 Subject: [PATCH 094/493] POSIX config: Fix order of dev IDs --- posix-configs/SITL/init/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index c89714c02c..319cf4214d 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -7,8 +7,8 @@ dataman start mavlink start -u 14556 -r 60000 simulator start -s param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 param set CAL_MAG0_ID 196608 param set CAL_GYRO0_XOFF 0.01 param set CAL_ACC0_XOFF 0.01 From 4a17411f8ffd8d7ee84c3e77d39097688587cab7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 19 Jun 2015 13:26:58 -0700 Subject: [PATCH 095/493] POSIX: Added systemcmds esc_cal and reboot modules Signed-off-by: Mark Charlebois --- makefiles/posix/config_posix_sitl.mk | 2 ++ makefiles/posix/posix_elf.mk | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index d0ec4086bb..10ae8ad3b3 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -20,6 +20,8 @@ MODULES += systemcmds/param MODULES += systemcmds/mixer MODULES += systemcmds/topic_listener MODULES += systemcmds/ver +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot # # General system control diff --git a/makefiles/posix/posix_elf.mk b/makefiles/posix/posix_elf.mk index edb1e32d76..bbc7545a5e 100644 --- a/makefiles/posix/posix_elf.mk +++ b/makefiles/posix/posix_elf.mk @@ -56,9 +56,11 @@ $(PRODUCT_SHARED_PRELINK): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(PRODUCT_SHARED_LIB): $(PRODUCT_SHARED_PRELINK) $(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK)) +$(WORK_DIR)apps.h: $(WORK_DIR)builtin_commands + $(PX4_BASE)/Tools/posix_apps.py > $(WORK_DIR)apps.h + MAIN = $(PX4_BASE)/src/platforms/posix/main.cpp -$(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) - $(PX4_BASE)/Tools/posix_apps.py > apps.h +$(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) $(WORK_DIR)apps.h $(call LINK,$@, -I. $(MAIN) $(PRODUCT_SHARED_LIB)) # From 45bae85daabaa64f297af399ee2d35e86cc7fd08 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 21 Jun 2015 00:31:23 +0300 Subject: [PATCH 096/493] ROMFS: Enable FTP on companion link Not sure that ftp usable in 57600. Tested on 921600. --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8a93737ef8..0fb6e117cc 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -479,11 +479,11 @@ then # but this works for now if param compare SYS_COMPANION 921600 then - mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 -x fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 -x fi if param compare SYS_COMPANION 157600 then From 66cdfeca6235f6ee7254e0d194e443ed257150d9 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 21 Jun 2015 00:31:23 +0300 Subject: [PATCH 097/493] ROMFS: Enable FTP on companion link Not sure that ftp usable in 57600. Tested on 921600. --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8a93737ef8..0fb6e117cc 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -479,11 +479,11 @@ then # but this works for now if param compare SYS_COMPANION 921600 then - mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 -x fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 -x fi if param compare SYS_COMPANION 157600 then From 8fa161b7c4947ccc988a737d2fd86610bca8cab2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 18:58:06 +0200 Subject: [PATCH 098/493] Multicopter configs: Remove duplicate defaults, each line checked to match new param-level defaults --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 32 ----------------------- 1 file changed, 32 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 1f34282aec..fa3653e0d5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -4,38 +4,6 @@ set VEHICLE_TYPE mc if [ $AUTOCNF == yes ] then - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.003 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.003 - param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAW_FF 0.5 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILTMAX_AIR 45.0 - param set MPC_TILTMAX_LND 15.0 - param set MPC_LAND_SPEED 1.0 - param set PE_VELNE_NOISE 0.5 param set PE_VELD_NOISE 0.7 param set PE_POSNE_NOISE 0.5 From 9365c5a4383d8702e66b5f0e3f95a1acb3fd5cac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 18:59:28 +0200 Subject: [PATCH 099/493] systemlib: Remove file present 2x from Makefile --- ROMFS/px4fmu_common/init.d/rc.usb | 6 +++--- src/modules/systemlib/module.mk | 3 +-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0442637941..027d2aca5d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,21 +3,21 @@ # USB MAVLink start # -mavlink start -r 80000 -d /dev/ttyACM0 -x +mavlink start -r 800000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100 mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 -mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 20 +mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100 mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20 # Exit shell to make it available to MAVLink diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f80d8009ae..f2499bbb13 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,8 +55,7 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.cpp \ circuit_breaker_params.c \ - mcu_version.c \ - circuit_breaker_params.c + mcu_version.c MAXOPTIMIZATION = -Os From 62b102d0b49f5cf47f5623761aed4db2a390ceaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 19:00:06 +0200 Subject: [PATCH 100/493] MC attitude controller: Set better defaults --- .../mc_att_control/mc_att_control_params.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) 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 c9bf3753b4..c0f110123a 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -50,7 +50,7 @@ * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); /** * Roll rate P gain @@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.05f); /** * Roll rate D gain @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); /** * Roll rate feedforward @@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); /** * Pitch rate P gain @@ -121,7 +121,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.05f); /** * Pitch rate D gain @@ -131,7 +131,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); /** * Pitch rate feedforward @@ -152,7 +152,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.8f); /** * Yaw rate P gain @@ -162,7 +162,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); /** * Yaw rate I gain @@ -172,7 +172,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); /** * Yaw rate D gain From 2c2a6b710c13f835d408a66104a47e73fb3685ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 19:00:23 +0200 Subject: [PATCH 101/493] MC position controller: Set better defaults --- src/modules/mc_pos_control/mc_pos_control_params.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 4865c1c684..a09ed4a3e6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -107,9 +107,10 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); * * @unit m/s * @min 0.0 + * @max 8 m/s * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); /** * Vertical velocity feed forward From dd9e3cd315761fc67ff5133046a34161de25a0fd Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 22 Jun 2015 09:40:45 +0200 Subject: [PATCH 102/493] call px4_open instead of open --- 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 d47b45d89f..eba0705ab5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -933,7 +933,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL"); } - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* vehicle status topic */ memset(&status, 0, sizeof(status)); From 80f1c517ccfae9d4bccda1e952b011ea12b34fd5 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 22 Jun 2015 09:41:05 +0200 Subject: [PATCH 103/493] init VDev for mavlink log device --- src/modules/mavlink/mavlink_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 619e076dfe..55aa2595f7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1528,7 +1528,11 @@ Mavlink::task_main(int argc, char *argv[]) #ifdef __PX4_NUTTX register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); #else - register_driver(MAVLINK_LOG_DEVICE, NULL); + int ret; + ret = VDev::init(); + if (ret != OK) { + PX4_WARN("VDev setup for mavlink log device failed!\n"); + } #endif /* initialize logging device */ From dfdc2c999da8b98db68e64f65c7cd6b0d03ff9b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:57:49 +0200 Subject: [PATCH 104/493] Bottle drop: Fix mavlink output --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index c00e73ecee..cd2897655d 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -338,7 +338,7 @@ void BottleDrop::task_main() { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); _command_sub = orb_subscribe(ORB_ID(vehicle_command)); From 82f3d4e87773132acbeab47cf64f82533136034c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:58:01 +0200 Subject: [PATCH 105/493] commander: Fix mavlink output --- src/modules/commander/commander.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d47b45d89f..eed18c95ad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -360,7 +360,7 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); int checkres = prearm_check(&status, mavlink_fd_local); close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); @@ -368,7 +368,7 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); arm_disarm(true, mavlink_fd_local, "command line"); warnx("note: not updating home position on commandline arming!"); close(mavlink_fd_local); @@ -376,7 +376,7 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "disarm")) { - int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); arm_disarm(false, mavlink_fd_local, "command line"); close(mavlink_fd_local); return 0; @@ -933,7 +933,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL"); } - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* vehicle status topic */ memset(&status, 0, sizeof(status)); @@ -1240,7 +1240,7 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { /* try to open the mavlink log device every once in a while */ - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); } arming_ret = TRANSITION_NOT_CHANGED; From 71fc0f5bc48141f2b93e7a3c39a00b5ddddc2b14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:58:11 +0200 Subject: [PATCH 106/493] EKF: Fix mavlink output --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 53c1b78e8a..3b447068c8 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -498,7 +498,7 @@ void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) void AttitudePositionEstimatorEKF::task_main() { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); From 46428769a5b9fa0a47fba4dfe3244ebb253e78fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:58:27 +0200 Subject: [PATCH 107/493] FW pos control: Fix mavlink output --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 48e78adaf0..ef6698ed5b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1648,7 +1648,7 @@ FixedwingPositionControl::task_main() /* XXX Hack to get mavlink output going */ if (_mavlink_fd < 0) { /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); } /* load local copies */ From 426b961abdfab762b6c8a2536dd19990f27244cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:58:40 +0200 Subject: [PATCH 108/493] MC pos control: Fix mavlink output --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 d92ee38414..891c827fea 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -898,7 +898,7 @@ void MulticopterPositionControl::task_main() { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* * do subscriptions From 6be1e7f7e85c41b1c4d3b298710bd4942f02b206 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:58:54 +0200 Subject: [PATCH 109/493] INAV: Fix mavlink output --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 53d6323e81..aedfe610b1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -216,7 +216,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e int position_estimator_inav_thread_main(int argc, char *argv[]) { int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel From 3e55e32098efcc4424a6e83eb8cfbe71df56c59b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:59:09 +0200 Subject: [PATCH 110/493] sdlog2: Fix mavlink output --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 270804075e..8548b90b56 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -868,7 +868,7 @@ bool copy_if_updated(orb_id_t topic, int *handle, void *buffer) int sdlog2_thread_main(int argc, char *argv[]) { - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { warnx("ERR: log stream, start mavlink app first"); From cc499fcc2943b5db85e657a417d752c6a8a76ea4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 23:02:09 +0200 Subject: [PATCH 111/493] Enable Q attitude estimator and INAV --- makefiles/posix/config_posix_sitl.mk | 2 ++ .../attitude_estimator_q_main.cpp | 35 +++++++++++-------- .../position_estimator_inav_main.c | 24 ++++++------- .../position_estimator_inav_params.c | 8 ++--- .../position_estimator_inav_params.h | 4 +-- 5 files changed, 39 insertions(+), 34 deletions(-) diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index d0ec4086bb..c317264143 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -31,6 +31,8 @@ MODULES += modules/mavlink # MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator +MODULES += modules/attitude_estimator_q +MODULES += modules/position_estimator_inav # # Vehicle Control diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f999a8f51b..9b945de915 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -39,7 +39,7 @@ * @author Anton Babushkin */ -#include +#include #include #include #include @@ -47,8 +47,6 @@ #include #include #include -#include -#include #include #include #include @@ -189,7 +187,7 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() { /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -206,7 +204,7 @@ int AttitudeEstimatorQ::start() { SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2500, - (main_t)&AttitudeEstimatorQ::task_main_trampoline, + (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -224,7 +222,7 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) { void AttitudeEstimatorQ::task_main() { warnx("started"); - _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -431,46 +429,53 @@ void AttitudeEstimatorQ::update(float dt) { int attitude_estimator_q_main(int argc, char *argv[]) { if (argc < 1) { - errx(1, "usage: attitude_estimator_q {start|stop|status}"); + warnx("usage: attitude_estimator_q {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { if (attitude_estimator_q::instance != nullptr) { - errx(1, "already running"); + warnx("already running"); + return 1; } attitude_estimator_q::instance = new AttitudeEstimatorQ; if (attitude_estimator_q::instance == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); + return 1; } if (OK != attitude_estimator_q::instance->start()) { delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; - err(1, "start failed"); + warnx("start failed"); + return 1; } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { if (attitude_estimator_q::instance == nullptr) { - errx(1, "not running"); + warnx("not running"); + return 1; } delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (attitude_estimator_q::instance) { - errx(0, "running"); + warnx("running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index aedfe610b1..a048013c21 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #include @@ -91,7 +90,6 @@ static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5 static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss -static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss static const uint32_t updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s @@ -121,7 +119,7 @@ static void usage(const char *reason) } fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); + return; } /** @@ -142,7 +140,7 @@ int position_estimator_inav_main(int argc, char *argv[]) if (thread_running) { warnx("already running"); /* this is not an error */ - exit(0); + return 0; } verbose_mode = false; @@ -157,7 +155,7 @@ int position_estimator_inav_main(int argc, char *argv[]) SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, (argv) ? (char * const *) &argv[2] : (char * const *) NULL); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { @@ -169,7 +167,7 @@ int position_estimator_inav_main(int argc, char *argv[]) warnx("not started"); } - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { @@ -180,11 +178,11 @@ int position_estimator_inav_main(int argc, char *argv[]) warnx("not started"); } - exit(0); + return 0; } usage("unrecognized command"); - exit(1); + return 1; } static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) @@ -194,7 +192,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e if (f) { char *s = malloc(256); unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", - hrt_absolute_time(), msg, (double)dt, + (unsigned long long)hrt_absolute_time(), msg, (double)dt, (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]); fwrite(s, 1, n, f); @@ -348,13 +346,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ - parameters_init(&pos_inav_param_handles); + inav_parameters_init(&pos_inav_param_handles); /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ - parameters_update(&pos_inav_param_handles, ¶ms); + inav_parameters_update(&pos_inav_param_handles, ¶ms); struct pollfd fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, @@ -428,7 +426,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - parameters_update(&pos_inav_param_handles, ¶ms); + inav_parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 382e9e46d7..eaa0b0a99e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -301,7 +301,7 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); */ PARAM_DEFINE_INT32(INAV_ENABLED, 1); -int parameters_init(struct position_estimator_inav_param_handles *h) +int inav_parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); @@ -326,10 +326,10 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->no_vision = param_find("CBRK_NO_VISION"); h->delay_gps = param_find("INAV_DELAY_GPS"); - return OK; + return 0; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); @@ -353,5 +353,5 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->no_vision, &(p->no_vision)); param_get(h->delay_gps, &(p->delay_gps)); - return OK; + return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 51bbda412a..7d348fb63c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -97,10 +97,10 @@ struct position_estimator_inav_param_handles { * Initialize all parameter handles and values * */ -int parameters_init(struct position_estimator_inav_param_handles *h); +int inav_parameters_init(struct position_estimator_inav_param_handles *h); /** * Update all parameters * */ -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); +int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); From 1d6f459e8c219ae0d88a3af4332e4cef438a5f66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 19:30:26 -0700 Subject: [PATCH 112/493] INAV: Disable verbose printing which created issues on POSIX. Needs further inspection --- .../position_estimator_inav_main.c | 57 ++++++++++--------- 1 file changed, 29 insertions(+), 28 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index a048013c21..372b9b06da 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -83,14 +83,14 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ -static bool verbose_mode = false; +static bool inav_verbose_mode = false; static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss -static const uint32_t updates_counter_len = 1000000; +static const unsigned updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -143,18 +143,18 @@ int position_estimator_inav_main(int argc, char *argv[]) return 0; } - verbose_mode = false; + inav_verbose_mode = false; if (argc > 1) if (!strcmp(argv[2], "-v")) { - verbose_mode = true; + inav_verbose_mode = true; } thread_should_exit = false; position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, - (argv) ? (char * const *) &argv[2] : (char * const *) NULL); + (argv && argc > 2) ? (char * const *) &argv[2] : (char * const *) NULL); return 0; } @@ -187,6 +187,7 @@ int position_estimator_inav_main(int argc, char *argv[]) static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { + return; FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a"); if (f) { @@ -354,7 +355,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); - struct pollfd fds_init[1] = { + px4_pollfd_struct_t fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, }; @@ -364,7 +365,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) thread_running = true; while (wait_baro && !thread_should_exit) { - int ret = poll(fds_init, 1, 1000); + int ret = px4_poll(fds_init, 1, 1000); if (ret < 0) { /* poll error */ @@ -398,12 +399,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* main loop */ - struct pollfd fds[1] = { + px4_pollfd_struct_t fds[1] = { { .fd = vehicle_attitude_sub, .events = POLLIN }, }; while (!thread_should_exit) { - int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate + int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -1071,25 +1072,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - if (verbose_mode) { - /* print updates rate */ - if (t > updates_counter_start + updates_counter_len) { - float updates_dt = (t - updates_counter_start) * 0.000001f; - warnx( - "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", - (double)(accel_updates / updates_dt), - (double)(baro_updates / updates_dt), - (double)(gps_updates / updates_dt), - (double)(attitude_updates / updates_dt), - (double)(flow_updates / updates_dt)); - updates_counter_start = t; - accel_updates = 0; - baro_updates = 0; - gps_updates = 0; - attitude_updates = 0; - flow_updates = 0; - } - } + // if (inav_verbose_mode) { + // /* print updates rate */ + // if (t > updates_counter_start + updates_counter_len) { + // float updates_dt = (t - updates_counter_start) * 0.000001f; + // warnx( + // "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", + // (double)(accel_updates / updates_dt), + // (double)(baro_updates / updates_dt), + // (double)(gps_updates / updates_dt), + // (double)(attitude_updates / updates_dt), + // (double)(flow_updates / updates_dt)); + // updates_counter_start = t; + // accel_updates = 0; + // baro_updates = 0; + // gps_updates = 0; + // attitude_updates = 0; + // flow_updates = 0; + // } + // } if (t > pub_last + PUB_INTERVAL) { pub_last = t; From 92d168a4765e8f77337ec72afa23abeaf75c8af8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 19:31:20 -0700 Subject: [PATCH 113/493] Q attitude estimator: Resolve POSIX porting issues: Add protection against bad input and output data --- .../attitude_estimator_q_main.cpp | 54 ++++++++++++++----- 1 file changed, 42 insertions(+), 12 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 9b945de915..8f250be131 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -146,6 +146,7 @@ private: hrt_abstime _vel_prev_t = 0; bool _inited = false; + bool _data_good = false; perf_counter_t _update_perf; perf_counter_t _loop_perf; @@ -154,9 +155,9 @@ private: int update_subscriptions(); - void init(); + bool init(); - void update(float dt); + bool update(float dt); }; @@ -220,7 +221,6 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) { } void AttitudeEstimatorQ::task_main() { - warnx("started"); _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -230,12 +230,12 @@ void AttitudeEstimatorQ::task_main() { hrt_abstime last_time = 0; - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = _sensors_sub; fds[0].events = POLLIN; while (!_task_should_exit) { - int ret = poll(fds, 1, 1000); + int ret = px4_poll(fds, 1, 1000); if (ret < 0) { // Poll error, sleep and try again @@ -254,6 +254,8 @@ void AttitudeEstimatorQ::task_main() { _gyro.set(sensors.gyro_rad_s); _accel.set(sensors.accelerometer_m_s2); _mag.set(sensors.magnetometer_ga); + + _data_good = true; } bool gpos_updated; @@ -289,7 +291,7 @@ void AttitudeEstimatorQ::task_main() { } // Time from previous iteration - uint64_t now = hrt_absolute_time(); + hrt_abstime now = hrt_absolute_time(); float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.0f; last_time = now; @@ -297,7 +299,9 @@ void AttitudeEstimatorQ::task_main() { dt = _dt_max; } - update(dt); + if (!update(dt)) { + continue; + } Vector<3> euler = _q.to_euler(); @@ -358,7 +362,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) { } } -void AttitudeEstimatorQ::init() { +bool AttitudeEstimatorQ::init() { // Rotation matrix can be easily constructed from acceleration and mag field vectors // 'k' is Earth Z axis (Down) unit vector in body frame Vector<3> k = -_accel; @@ -379,14 +383,31 @@ void AttitudeEstimatorQ::init() { // Convert to quaternion _q.from_dcm(R); + _q.normalize(); + + if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && + PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) && + _q.length() > 0.95f && _q.length() < 1.05f) { + _inited = true; + } else { + _inited = false; + } + + return _inited; } -void AttitudeEstimatorQ::update(float dt) { +bool AttitudeEstimatorQ::update(float dt) { if (!_inited) { - init(); - _inited = true; + + if (!_data_good) { + return false; + } + + return init(); } + Quaternion q_last = _q; + // Angular rate of correction Vector<3> corr; @@ -423,7 +444,16 @@ void AttitudeEstimatorQ::update(float dt) { _q += _q.derivative(corr) * dt; // Normalize quaternion - _q.normalize(); // TODO! NaN protection??? + _q.normalize(); + + if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && + PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) { + // Reset quaternion to last good state + _q = q_last; + return false; + } + + return true; } From 7df785ed50481ae29239b9289a7f73c77312fc84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 19:31:41 -0700 Subject: [PATCH 114/493] POSIX: Use the same estimators for multicopters as on the real system --- posix-configs/SITL/init/rcS | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 319cf4214d..c7b3ace95b 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -3,6 +3,8 @@ param load param set MAV_TYPE 2 param set MC_PITCHRATE_P 0.05 param set MC_ROLLRATE_P 0.05 +param set SYS_AUTOSTART 4010 +param set COM_RC_IN_MODE 2 dataman start mavlink start -u 14556 -r 60000 simulator start -s @@ -29,7 +31,8 @@ gpssim start hil mode_pwm commander start sensors start -ekf_att_pos_estimator 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 From 736125441ecb45d97f4bb2809eb3ff7d45eacc55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 20:02:20 -0700 Subject: [PATCH 115/493] POSIX: Allow unused variables in INAV estimator temporarily --- src/modules/position_estimator_inav/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 45c8762996..c92ba3007e 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -42,5 +42,5 @@ SRCS = position_estimator_inav_main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wframe-larger-than=3500 +EXTRACFLAGS = -Wframe-larger-than=3500 -Wno-unused From 8a3ac1f541de64fc64a2b7ee7760ebbb873d0cae Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 22 Jun 2015 13:47:11 +0200 Subject: [PATCH 116/493] set SYS_RESTART_TYPE in sitl startup, normally IO does that --- posix-configs/SITL/init/rcS | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index c7b3ace95b..e8f257195a 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -4,6 +4,7 @@ param set MAV_TYPE 2 param set MC_PITCHRATE_P 0.05 param set MC_ROLLRATE_P 0.05 param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 param set COM_RC_IN_MODE 2 dataman start mavlink start -u 14556 -r 60000 From 1c82f73822c1523be1bcaefd3b7986ddb8c59bdb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 22:15:45 +0200 Subject: [PATCH 117/493] Dataman: Reduce excessive stack allocation --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b442b74303..6cfbb4d830 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -794,7 +794,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1500, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From d673bf8457cdfe7fede5097e3b932645d07f1c62 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 22:16:03 +0200 Subject: [PATCH 118/493] Navigator: Reduce excessive stack allocation --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1460972cc2..dc97600434 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -521,7 +521,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, - 1700, + 1500, (main_t)&Navigator::task_main_trampoline, nullptr); From 75ec0267c9c73af3778c21eab2b4cc2415889c02 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Mon, 22 Jun 2015 23:33:22 +0100 Subject: [PATCH 119/493] mocap_support: update to debug log structure --- .../position_estimator_inav_main.c | 36 +++++++++++++------ 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index eaad4e3156..fb0c5f476d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -89,7 +89,7 @@ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s -static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s +static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms @@ -190,7 +190,9 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], + float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p, + float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v) { FILE *f = fopen("/fs/microsd/inav.log", "a"); @@ -201,10 +203,14 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]); fwrite(s, 1, n, f); - n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", + n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f mocap_pos_corr=[%.5f %.5f %.5f] w_mocap_p=%.5f\n", (double)acc[0], (double)acc[1], (double)acc[2], (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1], - (double)w_xy_gps_p, (double)w_xy_gps_v); + (double)w_xy_gps_p, (double)w_xy_gps_v, (double)corr_mocap[0][0], (double)corr_mocap[1][0], (double)corr_mocap[2][0], (double)w_mocap_p); + fwrite(s, 1, n, f); + n = snprintf(s, 256, "\tvision_pos_corr=[%.5f %.5f %.5f] vision_vel_corr=[%.5f %.5f %.5f] w_xy_vision_p=%.5f w_z_vision_p=%.5f w_xy_vision_v=%.5f\n", + (double)corr_vision[0][0], (double)corr_vision[1][0], (double)corr_vision[2][0], (double)corr_vision[0][1], (double)corr_vision[1][1], (double)corr_vision[2][1], + (double)w_xy_vision_p, (double)w_z_vision_p, (double)w_xy_vision_v); fwrite(s, 1, n, f); free(s); } @@ -300,9 +306,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float corr_mocap[3][1] = { - { 0.0f }, // N (pos) - { 0.0f }, // E (pos) - { 0.0f }, // D (pos) + { 0.0f }, // N (pos) + { 0.0f }, // E (pos) + { 0.0f }, // D (pos) }; float corr_sonar = 0.0f; @@ -1049,7 +1055,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, z_est, acc[2]); if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, + acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); } @@ -1074,7 +1082,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, + acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); @@ -1091,7 +1101,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, y_est, acc[1]); if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, + acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } @@ -1136,7 +1148,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, + acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, + corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); From ad1158b548a94e18de42966005f399e742b7d5e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:07:49 +0200 Subject: [PATCH 120/493] Fix F450 default gains --- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 9b3954be6f..2a77f13866 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -11,15 +11,15 @@ if [ $AUTOCNF == yes ] then # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.16 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.16 param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi From ff3977366607d1c54beb605029764b88261207b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:11:22 +0200 Subject: [PATCH 121/493] MC: Better attitude control defaults --- src/modules/mc_att_control/mc_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 c0f110123a..42c7bc3d04 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.12f); /** * Roll rate I gain @@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.12f); /** * Pitch rate I gain From ae9f1ec955906038652c459e6343b0d30f816722 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:11:34 +0200 Subject: [PATCH 122/493] CAN config: Better attitude control defaults --- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 05b4355138..c6341a4f74 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -9,18 +9,17 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then - # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.16 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.16 param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi From 4c975a11e5aa0402d3179e772d6580b71c4ae2de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:33:07 +0200 Subject: [PATCH 123/493] param command: Complete help text --- src/systemcmds/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 45fb2fd830..210d93bde8 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -189,7 +189,7 @@ param_main(int argc, char *argv[]) } } - errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare',\n'index', 'index_used', 'select' or 'save'"); } static void From c192398a6517648d3a46729c2ddf7268beed89a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:36:19 +0200 Subject: [PATCH 124/493] mavlink app: Be more verbose on param load fails --- src/modules/mavlink/mavlink_parameters.cpp | 23 +++++++++++++++++----- src/modules/mavlink/mavlink_parameters.h | 5 +++-- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 524effb205..73d7580d21 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * 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 @@ -36,6 +36,7 @@ * Mavlink parameters manager implementation. * * @author Anton Babushkin + * @author Lorenz Meier */ #include @@ -130,7 +131,17 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } else { /* when index is >= 0, send this parameter again */ - send_param(param_for_used_index(req_read.param_index)); + int ret = send_param(param_for_used_index(req_read.param_index)); + + if (ret == 1) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } else if (ret == 2) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } } } break; @@ -207,11 +218,11 @@ MavlinkParametersManager::send(const hrt_abstime t) } } -void +int MavlinkParametersManager::send_param(param_t param) { if (param == PARAM_INVALID) { - return; + return 1; } mavlink_param_value_t msg; @@ -221,7 +232,7 @@ MavlinkParametersManager::send_param(param_t param) * space during transmission, copy param onto float val_buf */ if (param_get(param, &msg.param_value) != OK) { - return; + return 2; } msg.param_count = param_count_used(); @@ -248,4 +259,6 @@ MavlinkParametersManager::send_param(param_t param) } _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); + + return 0; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index b6736f2128..3dfed084b3 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -36,6 +36,7 @@ * Mavlink parameters manager definition. * * @author Anton Babushkin + * @author Lorenz Meier */ #pragma once @@ -113,7 +114,7 @@ protected: void send(const hrt_abstime t); - void send_param(param_t param); + int send_param(param_t param); orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map; From f1582e67de83a4c5b30e22f5322b5686613c0580 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:36:42 +0200 Subject: [PATCH 125/493] F330 config: Better default gains --- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index d07e926a79..512ad132be 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -10,11 +10,11 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.13 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 From 7b588c5bd0a0b3c0cba09a16a3657ba44022b3fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:07:49 +0200 Subject: [PATCH 126/493] Fix F450 default gains --- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 9b3954be6f..2a77f13866 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -11,15 +11,15 @@ if [ $AUTOCNF == yes ] then # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.16 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.16 param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi From 26c47f25cb9613f1ceb9fffe04eef3d0cc70c293 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 10:37:56 +0200 Subject: [PATCH 127/493] PWM outputs: Allow the new p:PWM_OUT etc params for setting PWM limits via params at boot-time. --- src/modules/sensors/sensor_params.c | 83 +++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 6 +++ src/systemcmds/pwm/pwm.c | 32 +++++++++-- 3 files changed, 118 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7b3932638b..83568c0069 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1401,3 +1401,86 @@ PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000); * */ PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000); + +/** + * Enable Lidar-Lite (LL40LS) pwm driver + * + * @min 0 + * @max 1 + * @group Sensor Enable + */ +PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); + +/** + * Set the minimum PWM for the MAIN outputs + * + * Set to 1000 for default or 900 to increase servo travel + * + * @min 800 + * @max 1400 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MIN, 1000); + +/** + * Set the maximum PWM for the MAIN outputs + * + * Set to 2000 for default or 2100 to increase servo travel + * + * @min 1600 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAX, 2000); + +/** + * Set the disarmed PWM for MAIN outputs + * + * This is the PWM pulse the autopilot is outputting if not armed. + * The main use of this parameter is to silence ESCs when they are disarmed. + * + * @min 0 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_DISARMED, 0); + +/** + * Set the minimum PWM for the MAIN outputs + * + * Set to 1000 for default or 900 to increase servo travel + * + * @min 800 + * @max 1400 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000); + +/** + * Set the maximum PWM for the MAIN outputs + * + * Set to 2000 for default or 2100 to increase servo travel + * + * @min 1600 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); + +/** + * Set the disarmed PWM for AUX outputs + * + * This is the PWM pulse the autopilot is outputting if not armed. + * The main use of this parameter is to silence ESCs when they are disarmed. + * + * @min 0 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1000); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1e831becd9..0c7b0467f7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -634,6 +634,12 @@ Sensors::Sensors() : (void)param_find("CAL_MAG2_ROT"); (void)param_find("SYS_PARAM_VER"); (void)param_find("SYS_AUTOSTART"); + (void)param_find("PWM_MIN"); + (void)param_find("PWM_MAX"); + (void)param_find("PWM_DISARMED"); + (void)param_find("PWM_AUX_MIN"); + (void)param_find("PWM_AUX_MAX"); + (void)param_find("PWM_AUX_DISARMED"); /* fetch initial parameter values */ parameters_update(); diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 6bb9f235cb..168a1d8603 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -59,6 +59,7 @@ #include "systemlib/systemlib.h" #include "systemlib/err.h" +#include "systemlib/param/param.h" #include "drivers/drv_pwm_output.h" static void usage(const char *reason); @@ -187,10 +188,35 @@ pwm_main(int argc, char *argv[]) break; case 'p': - pwm_value = strtoul(optarg, &ep, 0); + { + /* check if this is a param name */ + if (strncmp("p:", optarg, 2) == 0) { - if (*ep != '\0') { - usage("BAD PWM VAL"); + char buf[32]; + strncpy(buf, optarg + 2, 16); + /* user wants to use a param name */ + param_t parm = param_find(buf); + + if (parm != PARAM_INVALID) { + int32_t pwm_parm; + int gret = param_get(parm, &pwm_parm); + + if (gret == 0) { + pwm_value = pwm_parm; + } else { + usage("PARAM LOAD FAIL"); + } + } else { + usage("PARAM NAME NOT FOUND"); + } + } else { + + pwm_value = strtoul(optarg, &ep, 0); + } + + if (*ep != '\0') { + usage("BAD PWM VAL"); + } } break; From 20d735701f6c4806d246e4d8fd75758f698b40bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 10:40:12 +0200 Subject: [PATCH 128/493] sensor params: Add hint to reboot system after changing PWM params --- src/modules/sensors/sensor_params.c | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 83568c0069..72d139b113 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1414,6 +1414,10 @@ PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); /** * Set the minimum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 1000 for default or 900 to increase servo travel * * @min 800 @@ -1426,6 +1430,10 @@ PARAM_DEFINE_INT32(PWM_MIN, 1000); /** * Set the maximum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 2000 for default or 2100 to increase servo travel * * @min 1600 @@ -1438,6 +1446,10 @@ PARAM_DEFINE_INT32(PWM_MAX, 2000); /** * Set the disarmed PWM for MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * This is the PWM pulse the autopilot is outputting if not armed. * The main use of this parameter is to silence ESCs when they are disarmed. * @@ -1451,6 +1463,10 @@ PARAM_DEFINE_INT32(PWM_DISARMED, 0); /** * Set the minimum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 1000 for default or 900 to increase servo travel * * @min 800 @@ -1463,6 +1479,10 @@ PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000); /** * Set the maximum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 2000 for default or 2100 to increase servo travel * * @min 1600 @@ -1475,6 +1495,10 @@ PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); /** * Set the disarmed PWM for AUX outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * This is the PWM pulse the autopilot is outputting if not armed. * The main use of this parameter is to silence ESCs when they are disarmed. * From 2284aa0c96d5abbe132931c7bcb91710c505fd24 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 00:47:20 +0200 Subject: [PATCH 129/493] Caipi config: Move to param based config --- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 1fd96d6d3d..8a661f25e2 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -34,7 +34,9 @@ then param set PWM_MAIN_REV1 1 fi +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX + set MIXER caipi -# Provide ESC a constant 1000 us pulse -set PWM_OUT 4 -set PWM_DISARMED 1000 +set PWM_OUT 1234 From 52b0f17ff31213e1c073cf53c069e8883a3ca0e9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 23 Jun 2015 12:29:25 +0200 Subject: [PATCH 130/493] increase highest pwm to 2150 --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 2fb9469c8a..18a75d063f 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -83,7 +83,7 @@ __BEGIN_DECLS /** * Highest maximum PWM in us */ -#define PWM_HIGHEST_MAX 2100 +#define PWM_HIGHEST_MAX 2150 /** * Default maximum PWM in us From 5d92927991f9375dccc125ef229a1dec33bf6f55 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 10:25:40 +0200 Subject: [PATCH 131/493] make motors spin in POSCTRL and ATTCTRL when landed and throttle applied by user --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e4682689af..392b31cf42 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -98,7 +98,8 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane #define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode -#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading +#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading +#define TAKEOFF_IDLE 0.1f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode @@ -1606,8 +1607,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = 0.0f; } else { /* Copy thrust and pitch values from tecs */ - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : - _tecs.get_throttle_demand(), throttle_max); + if (_vehicle_status.condition_landed && + (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) + { + // when we are landed in these modes we want the motor to spin + _att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max); + } else { + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max); + } + + } /* During a takeoff waypoint while waiting for launch the pitch sp is set From 6a00fce009528a99512cd6c2d0b105a2a97bef3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 19:55:04 +0200 Subject: [PATCH 132/493] EKF: Publish global position also if GPS is not yet valid so that controllers can get a valid altitude --- .../ekf_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 84da033adb..4208ed0deb 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -221,6 +221,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT"); + /* indicate consumers that the current position data is not valid */ + _gps.eph = 10000.0f; + _gps.epv = 10000.0f; + /* fetch initial parameter values */ parameters_update(); @@ -686,21 +690,21 @@ void AttitudePositionEstimatorEKF::task_main() continue; } - //Run EKF data fusion steps + // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); - //Publish attitude estimations + // Publish attitude estimations publishAttitude(); - //Publish Local Position estimations + // Publish Local Position estimations publishLocalPosition(); - //Publish Global Position, but only if it's any good - if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) { + // Publish Global Position, but only if it's any good + if (_gpsIsGood || _global_pos.dead_reckoning) { publishGlobalPosition(); } - //Publish wind estimates + // Publish wind estimates if (hrt_elapsed_time(&_wind.timestamp) > 99000) { publishWindEstimate(); } @@ -891,6 +895,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_utc_usec = _gps.time_utc_usec; + } else { + _global_pos.lat = 0.0; + _global_pos.lon = 0.0; + _global_pos.time_utc_usec = 0; } if (_local_pos.v_xy_valid) { @@ -907,6 +915,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; + } else { + _global_pos.vel_d = 0.0f; } /* terrain altitude */ From c46b4a29b8c0be96e40959305e52e6e753e555ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 20:00:38 +0200 Subject: [PATCH 133/493] EKF: Publish initial altitude estimate in any case --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4208ed0deb..877bff6585 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -699,10 +699,9 @@ void AttitudePositionEstimatorEKF::task_main() // Publish Local Position estimations publishLocalPosition(); - // Publish Global Position, but only if it's any good - if (_gpsIsGood || _global_pos.dead_reckoning) { - publishGlobalPosition(); - } + // Publish Global Position, it will have a large uncertainty + // set if only altitude is known + publishGlobalPosition(); // Publish wind estimates if (hrt_elapsed_time(&_wind.timestamp) > 99000) { From f4845b2b8f8b65d756dc37d51514a4c58c9cdd05 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:22:06 +0200 Subject: [PATCH 134/493] FW pos control: Guard against altitude estimate change --- .../fw_pos_control_l1_main.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 392b31cf42..79f25f3945 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -103,6 +103,7 @@ static int _control_task = -1; /**< task handle for sensor task */ static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode +static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; /** * L1 control app start / stop handling function @@ -174,7 +175,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for altitude mode */ - float _ground_alt; /**< ground altitude at which plane was launched */ + float _ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ @@ -969,9 +970,24 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) { const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; + // XXX this should go into a manual stick mapper + // class + static float _althold_epv = 0.0f; static bool was_in_deadband = false; bool climbout_mode = false; + /* + * Reset the hold altitude to the current altitude if the uncertainty + * changes significantly. + * This is to guard against uncommanded altitude changes + * when the altitude certainty increases or decreases. + */ + + if (fabsf(_althold_epv - _global_pos.epv) > ALTHOLD_EPV_RESET_THRESH) { + _hold_alt = _global_pos.alt; + _althold_epv = _global_pos.epv; + } + // XXX the sign magic in this function needs to be fixed if (_manual.x > deadBand) { @@ -988,6 +1004,7 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) * The aircraft should immediately try to fly at this altitude * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; + _althold_epv = _global_pos.epv; was_in_deadband = true; } From f680bbed545eea3a23b174bac4ccda4bf96b027d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:23:17 +0200 Subject: [PATCH 135/493] FW pos control: Rename _ground_alt to _takeoff_ground_alt to make it less ambigious with the actual terrain altitude --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 79f25f3945..90e4da3479 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -175,10 +175,10 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for altitude mode */ - float _ground_alt; /**< ground altitude at which plane was launched */ + float _takeoff_ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ - bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ + bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) { + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { return true; } @@ -1026,7 +1026,7 @@ void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitc { /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ if (in_takeoff_situation()) { - *hold_altitude = _ground_alt + _parameters.climbout_diff; + *hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff; *pitch_limit_min = math::radians(10.0f); } else { *pitch_limit_min = _parameters.pitch_limit_min; @@ -1068,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_in_air && !_vehicle_status.condition_landed) { _was_in_air = true; _time_went_in_air = hrt_absolute_time(); - _ground_alt = _global_pos.alt; + _takeoff_ground_alt = _global_pos.alt; } /* reset flag when airplane landed */ if (_vehicle_status.condition_landed) { From 5cf20c8dcfeba450bcc926f4a73b81c382a9ad43 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 23 Jun 2015 12:57:31 +0200 Subject: [PATCH 136/493] increase fw idle for ATTCTL and POSCTL to 0.2 --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 90e4da3479..95c8545e73 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -99,7 +99,7 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane #define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading -#define TAKEOFF_IDLE 0.1f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) +#define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode From 7043869237b5294233ca8dfaa613ceaaaf3d95bd Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 23 Jun 2015 18:32:40 +0200 Subject: [PATCH 137/493] VDev: - increase max number of devices to 200 - increase max number of file descriptors to 200 - add warning if number of file descriptor exceeds max value --- src/drivers/device/vdev.cpp | 2 +- src/drivers/device/vdev_posix.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index d992851309..0ed4d39ada 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -64,7 +64,7 @@ private: px4_dev_t() {} }; -#define PX4_MAX_DEV 100 +#define PX4_MAX_DEV 200 static px4_dev_t *devmap[PX4_MAX_DEV]; /* diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 975700d4e8..33aaa1647f 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -75,7 +75,7 @@ static void *timer_handler(void *data) return 0; } -#define PX4_MAX_FD 100 +#define PX4_MAX_FD 200 static device::file_t *filemap[PX4_MAX_FD] = {}; int px4_errno; @@ -117,6 +117,7 @@ int px4_open(const char *path, int flags, ...) ret = dev->open(filemap[i]); } else { + PX4_WARN("exceeded maximum number of file descriptors!"); ret = -ENOENT; } } From 51c8f64e9832b81b46bd4f38b701eb8e2f6501e1 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 23 Jun 2015 23:43:24 +0200 Subject: [PATCH 138/493] improve mavlink verbosity --- src/modules/mavlink/mavlink_main.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 55aa2595f7..c8427592d8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1673,15 +1673,28 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, - (double)_subscribe_to_stream_rate); + if ( get_protocol() == SERIAL ) { + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); + } else if ( get_protocol() == UDP ) { + warnx("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port, + (double)_subscribe_to_stream_rate); + } } else { - warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + if ( get_protocol() == SERIAL ) { + warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + } else if ( get_protocol() == UDP ) { + warnx("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port); + } } } else { - warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); + if ( get_protocol() == SERIAL ) { + warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); + } else if ( get_protocol() == UDP ) { + warnx("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port); + } } _subscribe_to_stream = nullptr; From 24ac4c9891c43b862f89b21c2c254791e4d81ced Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 24 Jun 2015 08:38:00 +0200 Subject: [PATCH 139/493] remove usleep in gyrosim --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 1ba888c65b..fa7f92abba 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -1070,23 +1070,17 @@ GYROSIM::measure() /* * Report buffers. */ - accel_report arb; + accel_report arb; gyro_report grb; - /* - * Adjust and scale results to m/s^2. - */ + // for now use local time but this should be the timestamp of the simulator grb.timestamp = hrt_absolute_time(); arb.timestamp = grb.timestamp; - - // this sleep is needed because the timing of the drivers is not yet working - usleep(1000); - // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = 0; + grb.error_count = arb.error_count = 0; // FIXME /* * 1) Scale raw value to SI units using scaling from datasheet. From ab550bcbbff60a4b19f38c71dcf98ae345d5ae97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 09:32:27 +0200 Subject: [PATCH 140/493] POSIX: Force shell to not immediately return --- src/platforms/posix/main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 475fa3e812..1899413ca0 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -69,6 +69,8 @@ static void run_cmd(const vector &appargs) { arg[i] = (char *)0; cout << "Running: " << command << "\n"; apps[command](i,(char **)arg); + // XXX hack to prevent shell returning too fast + usleep(250000); } else { From da29b88a04cd8f144ef48bf3b8b66652e5d46b91 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 24 Jun 2015 14:37:58 +0200 Subject: [PATCH 141/493] added LP filters (10Hz) on attitude rates in estimator --- .../AttitudePositionEstimatorEKF.h | 7 +++++++ .../ekf_att_pos_estimator_main.cpp | 16 ++++++++++------ 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 7084f716c0..0e991a51a5 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -66,6 +66,8 @@ #include #include +#include + #include #include @@ -258,6 +260,11 @@ private: AttPosEKF *_ekf; + /* Low pass filter for attitude rates */ + math::LowPassFilter2p _LP_att_P; + math::LowPassFilter2p _LP_att_Q; + math::LowPassFilter2p _LP_att_R; + private: /** * Update our local parameter cache. diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3b447068c8..89b4b0f47c 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -200,9 +200,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _newRangeData(false), _mavlink_fd(-1), - _parameters {}, - _parameter_handles {}, - _ekf(nullptr) + _parameters{}, + _parameter_handles{}, + _ekf(nullptr), + + _LP_att_P(100.0f, 10.0f), + _LP_att_Q(100.0f, 10.0f), + _LP_att_R(100.0f, 10.0f) { _last_run = hrt_absolute_time(); @@ -819,9 +823,9 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt; + _att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; From 66a637dcc731e27a6dfcfc0fd844ec35af03121d Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 24 Jun 2015 15:04:19 +0200 Subject: [PATCH 142/493] added covariances to estimator_status and logging --- msg/estimator_status.msg | 1 + .../ekf_att_pos_estimator_main.cpp | 5 +++ .../estimator_22states.cpp | 7 ++++ .../estimator_22states.h | 2 ++ src/modules/sdlog2/sdlog2.c | 14 ++++++++ src/modules/sdlog2/sdlog2_messages.h | 34 +++++++++++++------ 6 files changed, 53 insertions(+), 10 deletions(-) diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 92e5303a6a..ccbd2386db 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -4,3 +4,4 @@ float32 n_states # Number of states effectively used uint8 nan_flags # Bitmask to indicate NaN states uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt) +float32[28] covariances # Diagonal Elements of Covariance Matrix diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 89b4b0f47c..79b7afe5c7 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -478,8 +478,13 @@ int AttitudePositionEstimatorEKF::check_filter_state() size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + // Copy diagonal elemnts of covariance matrix + float covariances[28]; + _ekf->get_covariance(covariances); + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; + rep.covariances[i] = covariances[i]; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 5d56dbaae3..bf70ed13bc 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -3337,3 +3337,10 @@ void AttPosEKF::setIsFixedWing(const bool fixedWing) { _isFixedWing = fixedWing; } + +void AttPosEKF::get_covariance(float c[EKF_STATE_ESTIMATES]) +{ + for (unsigned int i = 0; i < EKF_STATE_ESTIMATES; i++) { + c[i] = P[i][i]; + } +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 9b23f4df44..426340d2ce 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -379,6 +379,8 @@ public: */ void ZeroVariables(); + void get_covariance(float c[28]); + protected: /** diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8548b90b56..ff8546bf94 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1126,6 +1126,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TEL_s log_TEL; struct log_EST0_s log_EST0; struct log_EST1_s log_EST1; + struct log_EST2_s log_EST2; + struct log_EST3_s log_EST3; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; struct log_VISN_s log_VISN; @@ -1845,6 +1847,18 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1); LOGBUFFER_WRITE_AND_COUNT(EST1); + + log_msg.msg_type = LOG_EST2_MSG; + unsigned maxcopy2 = (sizeof(buf.estimator_status.covariances) < sizeof(log_msg.body.log_EST2.cov)) ? sizeof(buf.estimator_status.covariances) : sizeof(log_msg.body.log_EST2.cov); + memset(&(log_msg.body.log_EST2.cov), 0, sizeof(log_msg.body.log_EST2.cov)); + memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2); + LOGBUFFER_WRITE_AND_COUNT(EST2); + + log_msg.msg_type = LOG_EST3_MSG; + unsigned maxcopy3 = ((sizeof(buf.estimator_status.covariances) - maxcopy2) < sizeof(log_msg.body.log_EST3.cov)) ? (sizeof(buf.estimator_status.covariances) - maxcopy2) : sizeof(log_msg.body.log_EST3.cov); + memset(&(log_msg.body.log_EST3.cov), 0, sizeof(log_msg.body.log_EST3.cov)); + memcpy(&(log_msg.body.log_EST3.cov), buf.estimator_status.covariances + maxcopy2, maxcopy3); + LOGBUFFER_WRITE_AND_COUNT(EST3); } /* --- TECS STATUS --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9cf37683ae..0929532e42 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -402,11 +402,23 @@ struct log_EST1_s { float s[16]; }; +/* --- EST2 - ESTIMATOR STATUS --- */ +#define LOG_EST2_MSG 34 +struct log_EST2_s { + float cov[12]; +}; + +/* --- EST3 - ESTIMATOR STATUS --- */ +#define LOG_EST3_MSG 35 +struct log_EST3_s { + float cov[16]; +}; + /* --- TEL0..3 - TELEMETRY STATUS --- */ -#define LOG_TEL0_MSG 34 -#define LOG_TEL1_MSG 35 -#define LOG_TEL2_MSG 36 -#define LOG_TEL3_MSG 37 +#define LOG_TEL0_MSG 36 +#define LOG_TEL1_MSG 37 +#define LOG_TEL2_MSG 38 +#define LOG_TEL3_MSG 39 struct log_TEL_s { uint8_t rssi; uint8_t remote_rssi; @@ -419,7 +431,7 @@ struct log_TEL_s { }; /* --- VISN - VISION POSITION --- */ -#define LOG_VISN_MSG 38 +#define LOG_VISN_MSG 40 struct log_VISN_s { float x; float y; @@ -434,7 +446,7 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCD_MSG 39 +#define LOG_ENCD_MSG 41 struct log_ENCD_s { int64_t cnt0; float vel0; @@ -443,22 +455,22 @@ struct log_ENCD_s { }; /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ -#define LOG_AIR1_MSG 41 +#define LOG_AIR1_MSG 42 /* --- VTOL - VTOL VEHICLE STATUS */ -#define LOG_VTOL_MSG 42 +#define LOG_VTOL_MSG 43 struct log_VTOL_s { float airspeed_tot; }; /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ -#define LOG_TSYN_MSG 43 +#define LOG_TSYN_MSG 44 struct log_TSYN_s { uint64_t time_offset; }; /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ -#define LOG_MACS_MSG 44 +#define LOG_MACS_MSG 45 struct log_MACS_s { float roll_rate_integ; float pitch_rate_integ; @@ -522,6 +534,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), 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(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"), From 0f21733cfc38edda892237bbb57deefd0de8b713 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 17:42:48 +0200 Subject: [PATCH 143/493] Wing-wing: Remove unused params. Camflyer: Copy wing-wing defaults --- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 24 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/3033_wingwing | 10 --------- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 1886783249..040b78dc77 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,6 +2,30 @@ sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MAX 15 + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 13 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.005 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.08 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.005 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.04 +fi + set MIXER Q # Provide ESC a constant 1000 us pulse while disarmed set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index add905b115..708c34491b 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -30,16 +30,6 @@ then param set FW_RR_I 0.005 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 - param set MT_TKF_PIT_MAX 30.0 - param set MT_ACC_D 0.2 - param set MT_ACC_P 0.6 - param set MT_A_LP 0.5 - param set MT_PIT_OFF 0.1 - param set MT_PIT_I 0.1 - param set MT_THR_OFF 0.65 - param set MT_THR_I 0.35 - param set MT_THR_P 0.2 - param set MT_THR_FF 1.5 fi set MIXER wingwing From 289ad91bcc2e49c33ae338152d7af19a13c4b64d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 17:44:11 +0200 Subject: [PATCH 144/493] Fixed wing land detector: Filter GPS speeds more since they are unreliable, leave airspeed filter where it was --- src/modules/land_detector/FixedwingLandDetector.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5f7ded9cb2..741bc02ad4 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -85,12 +85,12 @@ bool FixedwingLandDetector::update() bool landDetected = false; if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { - float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); if (isfinite(val)) { _velocity_xy_filtered = val; } - val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); if (isfinite(val)) { _velocity_z_filtered = val; From 640024357f3b3a261031b750cf7a7b5a82e53a78 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 17:44:44 +0200 Subject: [PATCH 145/493] Land detector: increase ground speed threshold --- src/modules/land_detector/land_detector_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index b670dcc035..f182495ac3 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -96,7 +96,7 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 4.0f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); /** * Fixedwing max climb rate From c2127f95010049e9bee24bcc874a31f332f480f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 08:46:26 +0200 Subject: [PATCH 146/493] mavlink app: Fix POSIX UDP transfer issues on larger packets --- src/modules/mavlink/mavlink_receiver.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8c4b16b49c..577e2bc794 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1626,7 +1626,13 @@ MavlinkReceiver::receive_thread(void *arg) { const int timeout = 500; +#ifdef __PX4_POSIX + /* 1500 is the Wifi MTU, so we make sure to fit a full packet */ + uint8_t buf[1600]; +#else + /* the serial port buffers internally as well, we just need to fit a small chunk */ uint8_t buf[32]; +#endif mavlink_message_t msg; struct pollfd fds[1]; From 3bad91dd3bd1d2ec712820c7b3f8f8b521cf8ac5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 09:28:04 +0200 Subject: [PATCH 147/493] systemlib: Fix param access for used params --- src/modules/systemlib/param/param.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 24bc9e73a6..c85e8dbda8 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -326,7 +326,8 @@ param_for_used_index(unsigned index) int count = get_param_info_count(); if (count && index < count) { - /* walk all params and count */ + /* walk all params and count used params */ + unsigned used_count = 0; for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { for (unsigned j = 0; j < bits_per_allocation_unit; j++) { @@ -335,11 +336,11 @@ param_for_used_index(unsigned index) /* we found the right used count, * return the param value */ - if (index == count) { + if (index == used_count) { return (param_t)(i * bits_per_allocation_unit + j); } - count++; + used_count++; } } } @@ -367,17 +368,17 @@ param_get_used_index(param_t param) } /* walk all params and count, now knowing that it has a valid index */ - int count = 0; + int used_count = 0; for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { for (unsigned j = 0; j < bits_per_allocation_unit; j++) { if (param_changed_storage[i] & (1 << j)) { if ((unsigned)param == i * bits_per_allocation_unit + j) { - return count; + return used_count; } - count++; + used_count++; } } } From 475c28803e20990ed33b96a03d6e540fb2bfe842 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 09:28:22 +0200 Subject: [PATCH 148/493] param command: Fix error handling if param is not found --- src/systemcmds/param/param.c | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 4aa1d185fd..9eaf3cabb7 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -62,8 +62,8 @@ __EXPORT int param_main(int argc, char *argv[]); static int do_save(const char *param_file_name); static int do_load(const char *param_file_name); static int do_import(const char *param_file_name); -static void do_show(const char *search_string); -static void do_show_index(const char *index, bool used_index); +static int do_show(const char *search_string); +static int do_show_index(const char *index, bool used_index); static void do_show_print(void *arg, param_t param); static int do_set(const char *name, const char *val, bool fail_on_not_found); static int do_compare(const char *name, char *vals[], unsigned comparisons); @@ -121,12 +121,10 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "show")) { if (argc >= 3) { - do_show(argv[2]); - return 0; + return do_show(argv[2]); } else { - do_show(NULL); - return 0; + return do_show(NULL); } } @@ -177,7 +175,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "index_used")) { if (argc >= 3) { - do_show_index(argv[2], true); + return do_show_index(argv[2], true); } else { warnx("no index provided"); return 1; @@ -186,7 +184,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "index")) { if (argc >= 3) { - do_show_index(argv[2], false); + return do_show_index(argv[2], false); } else { warnx("no index provided"); return 1; @@ -265,15 +263,17 @@ do_import(const char *param_file_name) return 0; } -static void +static int do_show(const char *search_string) { printf("Symbols: x = used, + = saved, * = unsaved\n"); param_foreach(do_show_print, (char *)search_string, false, false); printf("\n %u parameters total, %u used.\n", param_count(), param_count_used()); + + return 0; } -static void +static int do_show_index(const char *index, bool used_index) { char *end; @@ -289,7 +289,8 @@ do_show_index(const char *index, bool used_index) } if (param == PARAM_INVALID) { - return; + warnx("param not found for index %u", i); + return 1; } printf("index %d: %c %c %s [%d,%d] : ", i, (param_used(param) ? 'x' : ' '), @@ -314,7 +315,7 @@ do_show_index(const char *index, bool used_index) printf("\n", 0 + param_type(param)); } - exit(0); + return 0; } static void From 4fadb65ac65852bdf78534f7c50d289a1338eba1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 12:21:40 +0200 Subject: [PATCH 149/493] commander: Reject mag samples which are on top of each other --- src/modules/commander/mag_calibration.cpp | 109 ++++++++++++++++++---- 1 file changed, 92 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 04e66a5cb6..260316666c 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,6 +65,8 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; static const unsigned max_mags = 3; +static constexpr float mag_sphere_radius = 0.2f; +static const unsigned int calibration_sides = 3; calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); @@ -76,7 +78,7 @@ typedef struct { unsigned int calibration_points_perside; unsigned int calibration_interval_perside_seconds; uint64_t calibration_interval_perside_useconds; - unsigned int calibration_counter_total; + unsigned int calibration_counter_total[max_mags]; bool side_data_collected[detect_orientation_side_count]; float* x[max_mags]; float* y[max_mags]; @@ -184,6 +186,25 @@ int do_mag_calibration(int mavlink_fd) return result; } +static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count) +{ + float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; + //float min_sample_dist = (2.0f * M_PI_F * mag_sphere_radius / max_count) / 2.0f; + + for (size_t i = 0; i < count; i++) { + float dx = sx - x[i]; + float dy = sy - y[i]; + float dz = sz - z[i]; + float dist = sqrtf(dx * dx + dy * dy + dz * dz); + + if (dist < min_sample_dist) { + return true; + } + } + + return false; +} + static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { calibrate_return result = calibrate_return_ok; @@ -286,27 +307,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta int poll_ret = poll(fds, fd_count, 1000); if (poll_ret > 0) { + + int prev_count[max_mags]; + bool rejected = false; + for (size_t cur_mag=0; cur_magcalibration_counter_total[cur_mag]; + if (worker_data->sub_mag[cur_mag] >= 0) { struct mag_report mag; orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); + + // Check if this measurement is good to go in + rejected = rejected || reject_sample(mag.x, mag.y, mag.z, + worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], + worker_data->calibration_counter_total[cur_mag], + calibration_sides * worker_data->calibration_points_perside); - worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x; - worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; - worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; - + worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x; + worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y; + worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z; + worker_data->calibration_counter_total[cur_mag]++; } } - - worker_data->calibration_counter_total++; - calibration_counter_side++; - - // Progress indicator for side - mavlink_and_console_log_info(worker_data->mavlink_fd, - "[cal] %s side calibration: progress <%u>", - detect_orientation_str(orientation), - (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + + // Keep calibration of all mags in lockstep + if (rejected) { + // Reset counts, since one of the mags rejected the measurement + for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag]; + } + } else { + calibration_counter_side++; + + // Progress indicator for side + mavlink_and_console_log_info(worker_data->mavlink_fd, + "[cal] %s side calibration: progress <%u>", + detect_orientation_str(orientation), + (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + } } else { poll_errcount++; } @@ -336,7 +377,6 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; - worker_data.calibration_counter_total = 0; worker_data.calibration_points_perside = 80; worker_data.calibration_interval_perside_seconds = 20; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; @@ -357,9 +397,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.x[cur_mag] = NULL; worker_data.y[cur_mag] = NULL; worker_data.z[cur_mag] = NULL; + worker_data.calibration_counter_total[cur_mag] = 0; } - const unsigned int calibration_sides = 3; const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; char str[30]; @@ -438,7 +478,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag // Mag in this slot is available and we should have values for it to calibrate sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], - worker_data.calibration_counter_total, + worker_data.calibration_counter_total[cur_mag], 100, 0.0f, &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], &sphere_radius[cur_mag]); @@ -450,6 +490,41 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag } } } + + // Print uncalibrated data points + if (result == calibrate_return_ok) { + + printf("RAW DATA:\n--------------------\n"); + for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + + printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]); + + for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { + float x = worker_data.x[cur_mag][i]; + float y = worker_data.y[cur_mag][i]; + float z = worker_data.z[cur_mag][i]; + printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); + } + + printf(">>>>>>>\n"); + } + + printf("CALIBRATED DATA:\n--------------------\n"); + for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + + printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]); + + for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { + float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag]; + float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag]; + float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag]; + printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); + } + + printf("SPHERE RADIUS: %8.4f", (double)sphere_radius[cur_mag]); + printf(">>>>>>>\n"); + } + } // Data points are no longer needed for (size_t cur_mag=0; cur_mag Date: Wed, 24 Jun 2015 12:22:01 +0200 Subject: [PATCH 150/493] Tools: Add Matlab script to plot mag data --- Tools/Matlab/plot_mag.m | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 Tools/Matlab/plot_mag.m diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m new file mode 100644 index 0000000000..5fa4db4c23 --- /dev/null +++ b/Tools/Matlab/plot_mag.m @@ -0,0 +1,41 @@ +% +% Tool for plotting mag data +% +close all; +clear all; + +plot_scale = 0.8; + +xmax = plot_scale; +xmin = -xmax; +ymax = plot_scale; +ymin = -ymax; +zmax = plot_scale; +zmin = -zmax; + +mag0_raw = load('../../mag0_raw.csv'); +mag1_raw = load('../../mag1_raw.csv'); + +mag0_cal = load('../../mag0_cal.csv'); +mag1_cal = load('../../mag1_cal.csv'); + +fm0r = figure(); + +mag0_x_scale = 1.07; +mag0_y_scale = 0.95; +mag0_z_scale = 1.00; + +plot3(mag0_raw(:,1) .* mag0_x_scale, mag0_raw(:,2) .* mag0_y_scale, mag0_raw(:,3) .* mag0_z_scale, '*r'); +axis([xmin xmax ymin ymax zmin zmax]) + +fm1r = figure(); +plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); +axis([xmin xmax ymin ymax zmin zmax]) + +fm0c = figure(); +plot3(mag0_cal(:,1), mag0_cal(:,2), mag0_cal(:,3), '*b'); +axis([xmin xmax ymin ymax zmin zmax]) + +fm1c = figure(); +plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b'); +axis([xmin xmax ymin ymax zmin zmax]) From a4a6e69521658bae87597819274bd417110a44cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 08:42:59 +0200 Subject: [PATCH 151/493] Matlab tools: Add ellipsoid fit --- Tools/Matlab/ellipsoid_fit.m | 174 +++++++++++++++++++++++++++++++++++ Tools/Matlab/plot_mag.m | 31 +++++-- 2 files changed, 199 insertions(+), 6 deletions(-) create mode 100644 Tools/Matlab/ellipsoid_fit.m diff --git a/Tools/Matlab/ellipsoid_fit.m b/Tools/Matlab/ellipsoid_fit.m new file mode 100644 index 0000000000..d288aa3821 --- /dev/null +++ b/Tools/Matlab/ellipsoid_fit.m @@ -0,0 +1,174 @@ +% Copyright (c) 2009, Yury Petrov +% All rights reserved. +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are +% met: +% +% * Redistributions of source code must retain the above copyright +% notice, this list of conditions and the following disclaimer. +% * 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 +% +% 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. +% + +function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals ) +% +% Fit an ellispoid/sphere to a set of xyz data points: +% +% [center, radii, evecs, pars ] = ellipsoid_fit( X ) +% [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] ); +% [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 ); +% [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' ); +% [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 ); +% +% Parameters: +% * X, [x y z] - Cartesian data, n x 3 matrix or three n x 1 vectors +% * flag - 0 fits an arbitrary ellipsoid (default), +% - 1 fits an ellipsoid with its axes along [x y z] axes +% - 2 followed by, say, 'xy' fits as 1 but also x_rad = y_rad +% - 3 fits a sphere +% +% Output: +% * center - ellispoid center coordinates [xc; yc; zc] +% * ax - ellipsoid radii [a; b; c] +% * evecs - ellipsoid radii directions as columns of the 3x3 matrix +% * v - the 9 parameters describing the ellipsoid algebraically: +% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1 +% +% Author: +% Yury Petrov, Northeastern University, Boston, MA +% + +error( nargchk( 1, 3, nargin ) ); % check input arguments +if nargin == 1 + flag = 0; % default to a free ellipsoid +end +if flag == 2 && nargin == 2 + equals = 'xy'; +end + +if size( X, 2 ) ~= 3 + error( 'Input data must have three columns!' ); +else + x = X( :, 1 ); + y = X( :, 2 ); + z = X( :, 3 ); +end + +% need nine or more data points +if length( x ) < 9 && flag == 0 + error( 'Must have at least 9 points to fit a unique ellipsoid' ); +end +if length( x ) < 6 && flag == 1 + error( 'Must have at least 6 points to fit a unique oriented ellipsoid' ); +end +if length( x ) < 5 && flag == 2 + error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two axes equal' ); +end +if length( x ) < 3 && flag == 3 + error( 'Must have at least 4 points to fit a unique sphere' ); +end + +if flag == 0 + % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1 + D = [ x .* x, ... + y .* y, ... + z .* z, ... + 2 * x .* y, ... + 2 * x .* z, ... + 2 * y .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; % ndatapoints x 9 ellipsoid parameters +elseif flag == 1 + % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1 + D = [ x .* x, ... + y .* y, ... + z .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; % ndatapoints x 6 ellipsoid parameters +elseif flag == 2 + % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1, + % where A = B or B = C or A = C + if strcmp( equals, 'yz' ) || strcmp( equals, 'zy' ) + D = [ y .* y + z .* z, ... + x .* x, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; + elseif strcmp( equals, 'xz' ) || strcmp( equals, 'zx' ) + D = [ x .* x + z .* z, ... + y .* y, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; + else + D = [ x .* x + y .* y, ... + z .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; + end +else + % fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1 + D = [ x .* x + y .* y + z .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; % ndatapoints x 4 sphere parameters +end + +% solve the normal system of equations +v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) ); + +% find the ellipsoid parameters +if flag == 0 + % form the algebraic form of the ellipsoid + A = [ v(1) v(4) v(5) v(7); ... + v(4) v(2) v(6) v(8); ... + v(5) v(6) v(3) v(9); ... + v(7) v(8) v(9) -1 ]; + % find the center of the ellipsoid + center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ]; + % form the corresponding translation matrix + T = eye( 4 ); + T( 4, 1:3 ) = center'; + % translate to the center + R = T * A * T'; + % solve the eigenproblem + [ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) ); + radii = sqrt( 1 ./ diag( evals ) ); +else + if flag == 1 + v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ]; + elseif flag == 2 + if strcmp( equals, 'xz' ) || strcmp( equals, 'zx' ) + v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ]; + elseif strcmp( equals, 'yz' ) || strcmp( equals, 'zy' ) + v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ]; + else % xy + v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ]; + end + else + v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; + end + center = ( -v( 7:9 ) ./ v( 1:3 ) )'; + gam = 1 + ( v(7)^2 / v(1) + v(8)^2 / v(2) + v(9)^2 / v(3) ); + radii = ( sqrt( gam ./ v( 1:3 ) ) )'; + evecs = eye( 3 ); +end + + diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m index 5fa4db4c23..f5dbfc5edd 100644 --- a/Tools/Matlab/plot_mag.m +++ b/Tools/Matlab/plot_mag.m @@ -1,6 +1,19 @@ % % Tool for plotting mag data % +% Reference values: +% telem> [cal] mag #0 off: x:0.15 y:0.07 z:0.14 Ga +% MATLAB: x:0.1581 y: 0.0701 z: 0.1439 Ga +% telem> [cal] mag #0 scale: x:1.10 y:0.97 z:1.02 +% MATLAB: 0.5499, 0.5190, 0.4907 +% +% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga +% MATLAB: x:-0.1827 y:0.1147 z:-0.0848 Ga +% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 +% MATLAB: 0.5122, 0.5065, 0.4915 +% +% + close all; clear all; @@ -13,11 +26,11 @@ ymin = -ymax; zmax = plot_scale; zmin = -zmax; -mag0_raw = load('../../mag0_raw.csv'); -mag1_raw = load('../../mag1_raw.csv'); +mag0_raw = load('../../mag0_raw2.csv'); +mag1_raw = load('../../mag1_raw2.csv'); -mag0_cal = load('../../mag0_cal.csv'); -mag1_cal = load('../../mag1_cal.csv'); +mag0_cal = load('../../mag0_cal2.csv'); +mag1_cal = load('../../mag1_cal2.csv'); fm0r = figure(); @@ -25,15 +38,21 @@ mag0_x_scale = 1.07; mag0_y_scale = 0.95; mag0_z_scale = 1.00; -plot3(mag0_raw(:,1) .* mag0_x_scale, mag0_raw(:,2) .* mag0_y_scale, mag0_raw(:,3) .* mag0_z_scale, '*r'); +plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); +[center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); +center +radii axis([xmin xmax ymin ymax zmin zmax]) fm1r = figure(); plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); +[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); +center +radii axis([xmin xmax ymin ymax zmin zmax]) fm0c = figure(); -plot3(mag0_cal(:,1), mag0_cal(:,2), mag0_cal(:,3), '*b'); +plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); axis([xmin xmax ymin ymax zmin zmax]) fm1c = figure(); From cae604ac1f8177775048dacdc899d4372efaf0ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 08:43:34 +0200 Subject: [PATCH 152/493] HMC5883: Increase the number of calibration cycles to ensure a stable result --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5bf88da3df..22c8c6359c 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1124,8 +1124,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) } } - /* read the sensor up to 50x, stopping when we have 10 good values */ - for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + /* read the sensor up to 100x, stopping when we have 30 good values */ + for (uint8_t i = 0; i < 100 && good_count < 30; i++) { struct pollfd fds; /* wait for data to be ready */ From ef6092afd9c65d3937fe60e402f234eec5ffad38 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 10:44:19 +0200 Subject: [PATCH 153/493] HMC5883: Calculate correct scaling to apply using multiplication --- src/drivers/hmc5883/hmc5883.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 22c8c6359c..a9672dd7de 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1172,9 +1172,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) scaling[2] = sum_excited[2] / good_count; /* set scaling in device */ - mscale_previous.x_scale = scaling[0]; - mscale_previous.y_scale = scaling[1]; - mscale_previous.z_scale = scaling[2]; + mscale_previous.x_scale = 1.0f / scaling[0]; + mscale_previous.y_scale = 1.0f / scaling[1]; + mscale_previous.z_scale = 1.0f / scaling[2]; ret = OK; From e28e4cb84cf387c5749fce8548954e82dcfc4761 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 10:44:48 +0200 Subject: [PATCH 154/493] Matlab mag: Update to real scaling, resulting fits confirm results --- Tools/Matlab/plot_mag.m | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m index f5dbfc5edd..b344e41bc0 100644 --- a/Tools/Matlab/plot_mag.m +++ b/Tools/Matlab/plot_mag.m @@ -34,9 +34,9 @@ mag1_cal = load('../../mag1_cal2.csv'); fm0r = figure(); -mag0_x_scale = 1.07; -mag0_y_scale = 0.95; -mag0_z_scale = 1.00; +mag0_x_scale = 0.88; +mag0_y_scale = 0.99; +mag0_z_scale = 0.95; plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); [center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); @@ -53,6 +53,9 @@ axis([xmin xmax ymin ymax zmin zmax]) fm0c = figure(); plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); +[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); +center +radii axis([xmin xmax ymin ymax zmin zmax]) fm1c = figure(); From 1fdc6a922115c235e1164aef81c2487750f9cf5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 10:51:59 +0200 Subject: [PATCH 155/493] commander: Remove unused min sample dist --- src/modules/commander/mag_calibration.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 260316666c..7af023fd82 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -189,7 +189,6 @@ int do_mag_calibration(int mavlink_fd) static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count) { float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; - //float min_sample_dist = (2.0f * M_PI_F * mag_sphere_radius / max_count) / 2.0f; for (size_t i = 0; i < count; i++) { float dx = sx - x[i]; From 338404b4b395c21d75c0c5599610d4cebbfb0ef1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 24 Jun 2015 13:19:46 -0700 Subject: [PATCH 156/493] Change mag cal to 6 orientations --- src/modules/commander/calibration_messages.h | 2 +- src/modules/commander/mag_calibration.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 53775ffe4f..1dbc5b6dbf 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -49,7 +49,7 @@ // instead of visual calibration until such a time as QGC is update to the new version. // The number in the cal started message is used to indicate the version stamp for the current calibration code. -#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s" +#define CAL_QGC_STARTED_MSG "[cal] calibration started: 2 %s" #define CAL_QGC_DONE_MSG "[cal] calibration done: %s" #define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s" #define CAL_QGC_WARNING_MSG "[cal] calibration warning: %s" diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7af023fd82..36cc0cdd5d 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -66,7 +66,7 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; static const unsigned max_mags = 3; static constexpr float mag_sphere_radius = 0.2f; -static const unsigned int calibration_sides = 3; +static const unsigned int calibration_sides = 6; calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); @@ -376,7 +376,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; - worker_data.calibration_points_perside = 80; + worker_data.calibration_points_perside = 40; worker_data.calibration_interval_perside_seconds = 20; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; @@ -384,9 +384,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; - worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true; + worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false; for (size_t cur_mag=0; cur_mag>>>>>>\n"); } } From c402d0c2f7f856e8fd6b94be3ef2824a423f7843 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 13:20:43 +0200 Subject: [PATCH 157/493] Commander: updated mag calibration routine, matlab script updates --- Tools/Matlab/plot_mag.m | 34 +++++++++++++------ .../commander/calibration_routines.cpp | 6 ++-- 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m index b344e41bc0..c9f0c29925 100644 --- a/Tools/Matlab/plot_mag.m +++ b/Tools/Matlab/plot_mag.m @@ -13,6 +13,12 @@ % MATLAB: 0.5122, 0.5065, 0.4915 % % +% User-guided values: +% +% telem> [cal] mag #0 off: x:0.12 y:0.09 z:0.14 Ga +% telem> [cal] mag #0 scale: x:0.88 y:0.99 z:0.95 +% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga +% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 close all; clear all; @@ -26,11 +32,11 @@ ymin = -ymax; zmax = plot_scale; zmin = -zmax; -mag0_raw = load('../../mag0_raw2.csv'); -mag1_raw = load('../../mag1_raw2.csv'); +mag0_raw = load('../../mag0_raw3.csv'); +mag1_raw = load('../../mag1_raw3.csv'); -mag0_cal = load('../../mag0_cal2.csv'); -mag1_cal = load('../../mag1_cal2.csv'); +mag0_cal = load('../../mag0_cal3.csv'); +mag1_cal = load('../../mag1_cal3.csv'); fm0r = figure(); @@ -39,10 +45,11 @@ mag0_y_scale = 0.99; mag0_z_scale = 0.95; plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); -[center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); -center -radii +[mag0_raw_center, mag0_raw_radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); +mag0_raw_center +mag0_raw_radii axis([xmin xmax ymin ymax zmin zmax]) +viscircles([mag0_raw_center(1), mag0_raw_center(2)], [mag0_raw_radii(1)]); fm1r = figure(); plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); @@ -53,11 +60,18 @@ axis([xmin xmax ymin ymax zmin zmax]) fm0c = figure(); plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); -[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); -center -radii +[mag0_cal_center, mag0_cal_radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) .* mag0_x_scale mag1_raw(:,2) .* mag0_y_scale mag1_raw(:,3) .* mag0_z_scale] ); +mag0_cal_center +mag0_cal_radii axis([xmin xmax ymin ymax zmin zmax]) +viscircles([0, 0], [mag0_cal_radii(3)]); fm1c = figure(); plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b'); axis([xmin xmax ymin ymax zmin zmax]) +[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); +viscircles([0, 0], [radii(3)]); + +mag0_x_scale_matlab = 1 / (mag0_cal_radii(1) / mag0_raw_radii(1)) +mag0_y_scale_matlab = 1 / (mag0_cal_radii(2) / mag0_raw_radii(2)) +mag0_z_scale_matlab = 1 / (mag0_cal_radii(3) / mag0_raw_radii(3)) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index e9f83775d7..045dbb032d 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -243,7 +243,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub const float normal_still_thr = 0.25; // normal still threshold float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2); float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2 - hrt_abstime still_time = lenient_still_position ? 1000000 : 1500000; // still time required in us + hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us struct pollfd fds[1]; fds[0].fd = accel_sub; @@ -324,7 +324,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub /* not still, reset still start time */ if (t_still != 0) { mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); - usleep(500000); + usleep(200000); t_still = 0; } } @@ -488,7 +488,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, // Note that this side is complete side_data_collected[orient] = true; tune_neutral(true); - usleep(500000); + usleep(200000); } if (sub_accel >= 0) { From dcb680f9d6f16edee276df81b71a226164976a21 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 25 Jun 2015 22:04:23 +0200 Subject: [PATCH 158/493] VTOL: only publish attitude setpoint if in correct mode --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- .../mc_pos_control/mc_pos_control_main.cpp | 15 ++++++++++++--- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 95c8545e73..b5436602ec 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1762,11 +1762,11 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub > 0) { + if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); - } else { + } else if (_attitude_sp_pub <= 0 && !_vehicle_status.is_rotary_wing) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } 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 6eaca26b17..ccae79c11d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -119,6 +119,7 @@ private: int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ + int _vehicle_status_sub; /**< vehicle status subscription */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ @@ -134,6 +135,7 @@ private: orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ @@ -316,6 +318,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _reset_alt_sp(true), _mode_auto(false) { + memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); @@ -471,6 +474,12 @@ MulticopterPositionControl::poll_subscriptions() { bool updated; + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } + orb_check(_att_sub, &updated); if (updated) { @@ -900,6 +909,7 @@ MulticopterPositionControl::task_main() /* * do subscriptions */ + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1432,10 +1442,9 @@ MulticopterPositionControl::task_main() if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub > 0) { + if (_att_sp_pub > 0 && _vehicle_status.is_rotary_wing) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - - } else { + } else if (_att_sp_pub <= 0 && _vehicle_status.is_rotary_wing){ _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } From b1991b3813c00f28e657c44874e1dfc248019bb9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 25 Jun 2015 23:44:47 +0200 Subject: [PATCH 159/493] scale epv and eph correctly --- src/platforms/posix/drivers/gpssim/gpssim.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index 8b535e5992..9108a6f228 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -272,8 +272,8 @@ GPSSIM::receive(int timeout) { _report_gps_pos.lon = gps.lon; _report_gps_pos.alt = gps.alt; _report_gps_pos.timestamp_variance = hrt_absolute_time(); - _report_gps_pos.eph = (float)gps.eph; - _report_gps_pos.epv = (float)gps.epv; + _report_gps_pos.eph = (float)gps.eph * 1e-2f; + _report_gps_pos.epv = (float)gps.epv * 1e-2f; _report_gps_pos.vel_m_s = (float)(gps.vel)/100.0f; _report_gps_pos.vel_n_m_s = (float)(gps.vn)/100.0f; _report_gps_pos.vel_e_m_s = (float)(gps.ve)/100.0f; From f7a6afc976d696ecb6c28b5247ac823fe068d69c Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 00:40:02 +0200 Subject: [PATCH 160/493] improve SITL startup script --- posix-configs/SITL/init/rcS | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index e8f257195a..445b7ce339 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -22,6 +22,9 @@ 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 rgbled start tone_alarm start gyrosim start @@ -32,8 +35,13 @@ gpssim start hil mode_pwm commander start sensors start +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 stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 50 -s ATTITUDE -u 14556 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 From bd96f21ccbffafef469c1b79e32ae1ac3ad2fe03 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 00:41:07 +0200 Subject: [PATCH 161/493] fill local position setpoint message entirely --- src/modules/mavlink/mavlink_messages.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 24f04fd74f..5bc7c0492e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1776,6 +1776,10 @@ protected: msg.x = pos_sp.x; msg.y = pos_sp.y; msg.z = pos_sp.z; + msg.yaw = pos_sp.yaw; + msg.vx = pos_sp.vx; + msg.vy = pos_sp.vy; + msg.vz = pos_sp.vz; _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } From 51055a16fc4430913eabb43e63ea7fb10642e727 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Jun 2015 07:08:28 +0200 Subject: [PATCH 162/493] L3GD20: Set max offset for gyro to 25 degrees per second based on comment from mailing list about datasheet ratings. --- src/drivers/l3gd20/l3gd20.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 82c18b5c14..a05d285f7b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -179,6 +179,8 @@ static const int ERROR = -1; #define L3GD20_DEFAULT_FILTER_FREQ 30 #define L3GD20_TEMP_OFFSET_CELSIUS 40 +#define L3GD20_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */ + #ifdef PX4_SPI_BUS_EXT #define EXTERNAL_BUS PX4_SPI_BUS_EXT #else @@ -1102,18 +1104,18 @@ L3GD20::test_error() int L3GD20::self_test() { - /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + /* evaluate gyro offsets, complain if offset -> zero or larger than 25 dps */ + if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) return 1; From 572f1f4637926215d2ba24c960614d5ea578f2ac Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 00:41:07 +0200 Subject: [PATCH 163/493] fill local position setpoint message entirely --- src/modules/mavlink/mavlink_messages.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a333fb8529..0b6dfcab5f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1767,6 +1767,10 @@ protected: msg.x = pos_sp.x; msg.y = pos_sp.y; msg.z = pos_sp.z; + msg.yaw = pos_sp.yaw; + msg.vx = pos_sp.vx; + msg.vy = pos_sp.vy; + msg.vz = pos_sp.vz; _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } From 3defabfbea145242f7a78910241f114831046917 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 15:00:59 +0200 Subject: [PATCH 164/493] build land detector for posix --- makefiles/posix/config_posix_sitl.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index c317264143..806e59e7cb 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -40,6 +40,7 @@ MODULES += modules/position_estimator_inav MODULES += modules/navigator MODULES += modules/mc_pos_control MODULES += modules/mc_att_control +MODULES += modules/land_detector # # Library modules From 40cc11a5eda0161458a81f55d1c44a8f288655e1 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 15:01:17 +0200 Subject: [PATCH 165/493] ported land detector --- .../land_detector/FixedwingLandDetector.cpp | 4 +- .../land_detector/land_detector_main.cpp | 39 +++++++++++-------- 2 files changed, 25 insertions(+), 18 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5f7ded9cb2..b98f3fd4ce 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -87,12 +87,12 @@ bool FixedwingLandDetector::update() if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - if (isfinite(val)) { + if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - if (isfinite(val)) { + if (PX4_ISFINITE(val)) { _velocity_z_filtered = val; } } diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 1ca319ce63..58753d5ca7 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -38,6 +38,10 @@ * @author Johan Jansen */ +#include +#include +#include +#include #include //usleep #include #include @@ -80,7 +84,7 @@ static void land_detector_deamon_thread(int argc, char *argv[]) static void land_detector_stop() { if (land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); + warnx("not running"); return; } @@ -95,7 +99,7 @@ static void land_detector_stop() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_landDetectorTaskID); + px4_task_delete(_landDetectorTaskID); break; } } while (land_detector_task->isRunning()); @@ -104,7 +108,7 @@ static void land_detector_stop() delete land_detector_task; land_detector_task = nullptr; _landDetectorTaskID = -1; - errx(0, "land_detector has been stopped"); + warnx("land_detector has been stopped"); } /** @@ -113,7 +117,7 @@ static void land_detector_stop() static int land_detector_start(const char *mode) { if (land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); + warnx("already running"); return -1; } @@ -125,13 +129,13 @@ static int land_detector_start(const char *mode) land_detector_task = new MulticopterLandDetector(); } else { - errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); + warnx("[mode] must be either 'fixedwing' or 'multicopter'"); return -1; } //Check if alloc worked if (land_detector_task == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); return -1; } @@ -140,11 +144,11 @@ static int land_detector_start(const char *mode) SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1000, - (main_t)&land_detector_deamon_thread, + (px4_main_t)&land_detector_deamon_thread, nullptr); if (_landDetectorTaskID < 0) { - errx(1, "task start failed: %d", -errno); + warnx("task start failed: %d", -errno); return -1; } @@ -163,9 +167,9 @@ static int land_detector_start(const char *mode) usleep(50000); if (hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); + warnx("start failed - timeout"); land_detector_stop(); - exit(1); + return 1; } } printf("\n"); @@ -174,7 +178,6 @@ static int land_detector_start(const char *mode) //Remember current active mode strncpy(_currentMode, mode, 12); - exit(0); return 0; } @@ -189,12 +192,15 @@ int land_detector_main(int argc, char *argv[]) } if (argc >= 2 && !strcmp(argv[1], "start")) { - land_detector_start(argv[2]); + if (land_detector_start(argv[2]) != 0) { + warnx("land_detector start failed"); + return 1; + } } if (!strcmp(argv[1], "stop")) { land_detector_stop(); - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { @@ -204,13 +210,14 @@ int land_detector_main(int argc, char *argv[]) warnx("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); } else { - errx(1, "exists, but not running (%s)", _currentMode); + warnx("exists, but not running (%s)", _currentMode); } - exit(0); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } From c49511fb665127138bea698803067da1a9bdb5f3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 15:01:53 +0200 Subject: [PATCH 166/493] start land detector for SITL --- posix-configs/SITL/init/rcS | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 445b7ce339..d9f94c5ce0 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -35,6 +35,7 @@ gpssim start hil mode_pwm commander start sensors start +land_detector start multicopter navigator start attitude_estimator_q start position_estimator_inav start From 47c4ece9eb01eb66e97df2b968a9878da284a79c Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 15:02:12 +0200 Subject: [PATCH 167/493] use px4_poll instead of poll --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d7f971f067..e4ef373d1b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -309,7 +309,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[8]; + px4_pollfd_struct_t fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -332,7 +332,7 @@ Navigator::task_main() while (!_task_should_exit) { /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ From be69887b7e1c9543b8bcceb45d8c5f40fe4d759c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Jun 2015 19:50:49 +0200 Subject: [PATCH 168/493] EKF: Improved down gains from @boosfelm and @surberj --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_params.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index f8cca6c0dd..4702ec6cdb 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -141,13 +141,13 @@ PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f); /** * Velocity noise in down (vertical) direction * - * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7 + * Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7 * * @min 0.05 * @max 5.0 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f); +PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.3f); /** * Position noise in north-east (horizontal) direction @@ -163,13 +163,13 @@ PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f); /** * Position noise in down (vertical) direction * - * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0 + * Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0 * * @min 0.1 * @max 10.0 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f); +PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f); /** * Magnetometer measurement noise From 0fe01f6eb8f63a1487f1b6c19d193a1456ebf3b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 10:17:15 +0200 Subject: [PATCH 169/493] MC pos control: Fix manual yaw handling to not reset yaw in extreme angle conditions --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 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 ccae79c11d..9f275cd8fe 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1392,16 +1392,16 @@ MulticopterPositionControl::task_main() // do not move yaw while arming else if (_manual.z > 0.1f) { - const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; + 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; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); - float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); - if (yaw_offs < - YAW_OFFSET_MAX) { - _att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX); + float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); + float yaw_offs = _wrap_pi(yaw_target - _att.yaw); - } else if (yaw_offs > YAW_OFFSET_MAX) { - _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); + // If the yaw offset became too big for the system to track stop + // shifting it + if (fabsf(yaw_offs) < yaw_offset_max) { + _att_sp.yaw_body = yaw_target; } } From ba68b70b0b6dd40f4c68075e909bcb251029621a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 10:53:00 +0200 Subject: [PATCH 170/493] MC pos control: Comment style fixes --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ccae79c11d..2166541ccf 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1380,16 +1380,16 @@ MulticopterPositionControl::task_main() reset_int_xy = true; } - // generate attitude setpoint from manual controls + /* generate attitude setpoint from manual controls */ if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { - // reset yaw setpoint to current position if needed + /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { reset_yaw_sp = false; _att_sp.yaw_body = _att.yaw; } - // do not move yaw while arming + /* do not move yaw while arming */ else if (_manual.z > 0.1f) { const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; @@ -1405,19 +1405,19 @@ MulticopterPositionControl::task_main() } } - //Control roll and pitch directly if we no aiding velocity controller is active + /* control roll and pitch directly if we no aiding velocity controller is active */ if (!_control_mode.flag_control_velocity_enabled) { _att_sp.roll_body = _manual.y * _params.man_roll_max; _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; } - //Control climb rate directly if no aiding altitude controller is active + /* control throttle directly if no climb rate controller is active */ if (!_control_mode.flag_control_climb_rate_enabled) { _att_sp.thrust = math::min(_manual.z, _params.thr_max); _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); } - //Construct attitude setpoint rotation matrix + /* construct attitude setpoint rotation matrix */ math::Matrix<3,3> R_sp; R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); From 4d4f3cffefcf18a525c1e8013906d08d0039e416 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 11:13:05 +0200 Subject: [PATCH 171/493] Update FW EKF default params --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 156711c26d..b718f421f5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -17,9 +17,9 @@ then param set FW_T_TIME_CONST 5 param set PE_VELNE_NOISE 0.3 - param set PE_VELD_NOISE 0.5 + param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 0.5 + param set PE_POSD_NOISE 1.0 param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0002 fi From 4c70fadb38c880821e6f2af11ac36eeed989e721 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 11:13:18 +0200 Subject: [PATCH 172/493] Update MC EKF default params --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index fa3653e0d5..a5c326ebc6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -5,9 +5,9 @@ set VEHICLE_TYPE mc if [ $AUTOCNF == yes ] then param set PE_VELNE_NOISE 0.5 - param set PE_VELD_NOISE 0.7 + param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 1.0 + param set PE_POSD_NOISE 1.25 param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0001 From cf4778b662de34f829269d06c9339cc1859bf930 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 11:13:37 +0200 Subject: [PATCH 173/493] Update VTOL EKF default params --- ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 844d540bf0..c2340a7b16 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -34,9 +34,9 @@ then param set FW_RR_P 0.02 param set PE_VELNE_NOISE 0.5 - param set PE_VELD_NOISE 0.7 + param set PE_VELD_NOISE 0.3 param set PE_POSNE_NOISE 0.5 - param set PE_POSD_NOISE 1.0 + param set PE_POSD_NOISE 1.25 param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0001 fi From 5e27b7bc31593b3057bcceb16eb071128e1d2eaf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 27 Jun 2015 21:23:53 +1000 Subject: [PATCH 174/493] ms5611: fixed the i2c device on NuttX. This was left-over debugging noise from the POSIX bringup. --- src/drivers/ms5611/ms5611_i2c.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 730fd9b968..11baafd23b 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -71,8 +71,8 @@ public: virtual ~MS5611_I2C(); virtual int init(); - virtual int dev_read(unsigned offset, void *data, unsigned count); - virtual int dev_ioctl(unsigned operation, unsigned &arg); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); #ifdef __PX4_NUTTX protected: @@ -139,7 +139,7 @@ MS5611_I2C::init() } int -MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count) +MS5611_I2C::read(unsigned offset, void *data, unsigned count) { union _cvt { uint8_t b[4]; @@ -162,7 +162,7 @@ MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count) } int -MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg) +MS5611_I2C::ioctl(unsigned operation, unsigned &arg) { int ret; From b02c4ec3562c1713fee0bc4d4a55eddf22df63b0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 27 Jun 2015 18:12:18 +0200 Subject: [PATCH 175/493] add max vel constraints to multiplatform MPC --- .../mc_pos_control.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index e0e086999d..37775711e0 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -647,6 +647,21 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + /* make sure velocity setpoint is saturated in xy*/ + float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + + _vel_sp(1)*_vel_sp(1)); + if (vel_norm_xy > _params.vel_max(0)) { + /* note assumes vel_max(0) == vel_max(1) */ + _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; + _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + } + + /* make sure velocity setpoint is saturated in z*/ + float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { + _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; + } + if (!_control_mode->data().flag_control_altitude_enabled) { _reset_alt_sp = true; _vel_sp(2) = 0.0f; From a97931bf20d7e88caf89d475284339ce7e49300b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 15:46:59 +0200 Subject: [PATCH 176/493] Update orb advert type in commander, by @boosfelm --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index af32a067b9..de7f1694cc 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -368,7 +368,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta /** * Transition from one hil state to another */ -transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { transition_result_t ret = TRANSITION_DENIED; From 6638729af78be2eb99084f636fcfc01bcdf3c8e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 15:47:19 +0200 Subject: [PATCH 177/493] Update orb advert type in mavlink, by @boosfelm --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 36ac721d91..f143c08aae 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1009,7 +1009,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) rc.values[6] = man.chan7_raw; rc.values[7] = man.chan8_raw; - if (_rc_pub <= 0) { + if (_rc_pub == nullptr) { _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); } else { From 5b354b96310fb5fed71bfa944db00052c3ad5bfa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 16:10:40 +0200 Subject: [PATCH 178/493] PWM driver: Fix _IOC to _PX4_IOC for getting servo rate --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 83c8618b75..eda113f5e4 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -156,7 +156,7 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_DISARM _PX4_IOC(_PWM_SERVO_BASE, 1) /** get default servo update rate */ -#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 2) /** set alternate servo update rate */ #define PWM_SERVO_SET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 3) From 93580da922fff6268effa03cfa4cfaa00c1537f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 16:24:34 +0200 Subject: [PATCH 179/493] commander: Restructure ifdef logic for POSIX build to keep NuttX and POSIX implementations aligned --- .../commander/state_machine_helper.cpp | 62 ++++++++++--------- 1 file changed, 33 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index de7f1694cc..984046ee03 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -451,43 +451,47 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta } closedir(d); -#else - - const char *devname; - unsigned int handle = 0; - for(;;) { - devname = px4_get_device_names(&handle); - if (devname == NULL) - break; - - /* skip mavlink */ - if (!strcmp("/dev/mavlink", devname)) { - continue; - } - - - int sensfd = px4_open(devname, 0); - - if (sensfd < 0) { - warn("failed opening device %s", devname); - continue; - } - - int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - px4_close(sensfd); - - printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); - } -#endif ret = TRANSITION_CHANGED; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - } else { /* failed opening dir */ mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); ret = TRANSITION_DENIED; } + +#else + + const char *devname; + unsigned int handle = 0; + for(;;) { + devname = px4_get_device_names(&handle); + if (devname == NULL) + break; + + /* skip mavlink */ + if (!strcmp("/dev/mavlink", devname)) { + continue; + } + + + int sensfd = px4_open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } + + int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + px4_close(sensfd); + + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); +#endif + } else { mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; From ac6abacac9d8c44d4f0407b75fc30424f38b22c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 16:28:33 +0200 Subject: [PATCH 180/493] VTOL controller: Fix usage of old uORB API, fix indentation --- .../vtol_att_control_main.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 1c76c25505..589a08d8cd 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -75,7 +75,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _actuators_0_pub(nullptr), _actuators_1_pub(nullptr), _vtol_vehicle_status_pub(nullptr), - _v_rates_sp_pub(nullptr), + _v_rates_sp_pub(nullptr) { memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); @@ -540,23 +540,23 @@ void VtolAttitudeControl::task_main() /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) - { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) + { + if (_actuators_0_pub != nullptr) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); } + if (_actuators_1_pub != nullptr) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } + } + // publish the attitude rates setpoint if(_v_rates_sp_pub != nullptr) { orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp); From 6cf47b59da3cb7f19d493a2706a5e7aa39c90817 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 15:46:14 +0200 Subject: [PATCH 181/493] FW controller: Update use to new uORB API --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5f1c64b106..ccb735e941 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1766,7 +1766,7 @@ FixedwingPositionControl::task_main() /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); - } else if (_attitude_sp_pub <= 0 && !_vehicle_status.is_rotary_wing) { + } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } From 064c02a81730ab34a8a5487a343ef8982c38c816 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 15:46:40 +0200 Subject: [PATCH 182/493] MC controller: Update use to new uORB API --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 6d866148e4..1dc2c22d39 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1447,7 +1447,7 @@ MulticopterPositionControl::task_main() _control_mode.flag_control_velocity_enabled))) { if (_att_sp_pub != nullptr && _vehicle_status.is_rotary_wing) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - } else if (_att_sp_pub <= 0 && _vehicle_status.is_rotary_wing){ + } else if (_att_sp_pub == nullptr && _vehicle_status.is_rotary_wing){ _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } From dfae432f1a15476f6a52ffa52a406a0251d22fc2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 11:55:02 -0700 Subject: [PATCH 183/493] commander: Fix mag cal printing --- src/modules/commander/mag_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index bd091ac13c..8e11f3e65e 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -499,7 +499,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag printf("RAW DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { - printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]); + printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { float x = worker_data.x[cur_mag][i]; @@ -514,7 +514,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag printf("CALIBRATED DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { - printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]); + printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag]; From cddfcb35d8132be466188c4bd695980d3a760d0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 11:55:21 -0700 Subject: [PATCH 184/493] Posix main: Only delay app startup 50 ms --- src/platforms/posix/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 1899413ca0..7582183111 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -70,7 +70,7 @@ static void run_cmd(const vector &appargs) { cout << "Running: " << command << "\n"; apps[command](i,(char **)arg); // XXX hack to prevent shell returning too fast - usleep(250000); + usleep(50000); } else { From 60b8c28be2d3fabb3d49aa0fd8a37fed26b9ed96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jun 2015 13:35:55 +0200 Subject: [PATCH 185/493] INAV app: Fix commandline handling --- .../position_estimator_inav_main.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 7af3a355eb..e0d7844d95 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -147,10 +147,9 @@ int position_estimator_inav_main(int argc, char *argv[]) verbose_mode = false; - if (argc > 1) - if (!strcmp(argv[2], "-v")) { - verbose_mode = true; - } + if (argc > 2 && !strcmp(argv[2], "-v")) { + verbose_mode = true; + } thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", From 0c1ec5eb8b11a5e6b10ff852e0591ddce3ff9b9c Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 28 Jun 2015 15:15:50 +0100 Subject: [PATCH 186/493] makefiles: add /dev/serial/by-id/pci-3D_Robotics* to the ports to try on Linux --- makefiles/nuttx/upload.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/nuttx/upload.mk b/makefiles/nuttx/upload.mk index c590f17d10..e73c31dc31 100644 --- a/makefiles/nuttx/upload.mk +++ b/makefiles/nuttx/upload.mk @@ -15,7 +15,7 @@ ifeq ($(SYSTYPE),Darwin) SERIAL_PORTS ?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" endif ifeq ($(SYSTYPE),Linux) -SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*" +SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/pci-3D_Robotics*" endif ifeq ($(SERIAL_PORTS),) SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" From b0642f8d32b3a2ec509b49d74832d651d5e0cf01 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 28 Jun 2015 15:24:48 +0100 Subject: [PATCH 187/493] land_detector: shut up if started correctly --- src/modules/land_detector/land_detector_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 58753d5ca7..34355c6b11 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -196,6 +196,7 @@ int land_detector_main(int argc, char *argv[]) warnx("land_detector start failed"); return 1; } + return 0; } if (!strcmp(argv[1], "stop")) { From 99d59971acbe9e1ecdf9ee3f1318005bb19fcd1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jun 2015 13:35:55 +0200 Subject: [PATCH 188/493] INAV app: Fix commandline handling Conflicts: src/modules/position_estimator_inav/position_estimator_inav_main.c --- .../position_estimator_inav/position_estimator_inav_main.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 372b9b06da..6f60e4878d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -145,10 +145,9 @@ int position_estimator_inav_main(int argc, char *argv[]) inav_verbose_mode = false; - if (argc > 1) - if (!strcmp(argv[2], "-v")) { - inav_verbose_mode = true; - } + if (argc > 2 && !strcmp(argv[2], "-v")) { + inav_verbose_mode = true; + } thread_should_exit = false; position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", From 5523b1ee4f118cb07794e2394d029b09eb4e595e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jun 2015 16:29:46 +0200 Subject: [PATCH 189/493] Re-enable INAV verbose options --- .../position_estimator_inav_main.c | 38 +++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 6f60e4878d..327276c5c1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1071,25 +1071,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - // if (inav_verbose_mode) { - // /* print updates rate */ - // if (t > updates_counter_start + updates_counter_len) { - // float updates_dt = (t - updates_counter_start) * 0.000001f; - // warnx( - // "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", - // (double)(accel_updates / updates_dt), - // (double)(baro_updates / updates_dt), - // (double)(gps_updates / updates_dt), - // (double)(attitude_updates / updates_dt), - // (double)(flow_updates / updates_dt)); - // updates_counter_start = t; - // accel_updates = 0; - // baro_updates = 0; - // gps_updates = 0; - // attitude_updates = 0; - // flow_updates = 0; - // } - // } + if (inav_verbose_mode) { + /* print updates rate */ + if (t > updates_counter_start + updates_counter_len) { + float updates_dt = (t - updates_counter_start) * 0.000001f; + warnx( + "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", + (double)(accel_updates / updates_dt), + (double)(baro_updates / updates_dt), + (double)(gps_updates / updates_dt), + (double)(attitude_updates / updates_dt), + (double)(flow_updates / updates_dt)); + updates_counter_start = t; + accel_updates = 0; + baro_updates = 0; + gps_updates = 0; + attitude_updates = 0; + flow_updates = 0; + } + } if (t > pub_last + PUB_INTERVAL) { pub_last = t; From 74d95f0441b55a57f9730e80afc8171a5edd1ee8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jun 2015 16:31:04 +0200 Subject: [PATCH 190/493] INAV: Remove extra C++ flag --- src/modules/position_estimator_inav/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index c92ba3007e..45c8762996 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -42,5 +42,5 @@ SRCS = position_estimator_inav_main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wframe-larger-than=3500 -Wno-unused +EXTRACFLAGS = -Wframe-larger-than=3500 From abc069dc134e8bb3e6a1404c1b937f13f42a387f Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 28 Jun 2015 15:15:50 +0100 Subject: [PATCH 191/493] makefiles: add /dev/serial/by-id/pci-3D_Robotics* to the ports to try on Linux --- makefiles/upload.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index dd7710bf72..2da09bd980 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -15,7 +15,7 @@ ifeq ($(SYSTYPE),Darwin) SERIAL_PORTS ?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" endif ifeq ($(SYSTYPE),Linux) -SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*" +SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/pci-3D_Robotics*" endif ifeq ($(SERIAL_PORTS),) SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" From e9d597816518030572a3c5507c1067c84dbbba40 Mon Sep 17 00:00:00 2001 From: cctsao1008 Date: Tue, 30 Jun 2015 00:21:47 +0800 Subject: [PATCH 192/493] Adjust the duration of the BIND pulse Some DSMX Remote Receiver can't enter BIND mode with the duration about 25us but 120us. --- src/modules/px4iofirmware/dsm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index afde16ed39..b0e96b1f03 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -292,9 +292,9 @@ dsm_bind(uint16_t cmd, int pulses) /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { - up_udelay(25); + up_udelay(120); stm32_gpiowrite(usart1RxAsOutp, false); - up_udelay(25); + up_udelay(120); stm32_gpiowrite(usart1RxAsOutp, true); } break; From da2ac877f829db1339948bcadd29bcbc6a9970eb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 29 Jun 2015 19:08:06 -0700 Subject: [PATCH 193/493] POSIX: Changed px4_poll to use hrt_work queue QuRT's pthread_cancel implementation is lacking, and causes px4_poll to always wait for the maximumn timeout. A cleaner implementation is provided that uses the HRT work queue for posix targets. In the future the posix code should be rtefactiored so that qurt (and other) implementations that are duplicated, use the posix implementation. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev_posix.cpp | 22 +++++-------------- .../posix/{px4_layer => include}/hrt_work.h | 5 +---- .../qurt/{px4_layer => include}/hrt_work.h | 2 +- 3 files changed, 8 insertions(+), 21 deletions(-) rename src/platforms/posix/{px4_layer => include}/hrt_work.h (95%) rename src/platforms/qurt/{px4_layer => include}/hrt_work.h (99%) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 33aaa1647f..f9a6ebc559 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -43,6 +43,7 @@ #include "device.h" #include "vfile.h" +#include #include #include #include @@ -61,7 +62,7 @@ struct timerData { ~timerData() {} }; -static void *timer_handler(void *data) +static void timer_cb(void *data) { struct timerData *td = (struct timerData *)data; @@ -72,7 +73,6 @@ static void *timer_handler(void *data) sem_post(&(td->sem)); PX4_DEBUG("timer_handler: Timer expired"); - return 0; } #define PX4_MAX_FD 200 @@ -239,25 +239,16 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { if (timeout >= 0) { - pthread_t pt; - void *res; + // Use a work queue task + work_s _hpwork; - ts.tv_sec = timeout/1000; - ts.tv_nsec = (timeout % 1000)*1000000; - - // Create a timer to unblock struct timerData td(sem, ts); - int rv = pthread_create(&pt, NULL, timer_handler, (void *)&td); - if (rv != 0) { - count = -1; - goto cleanup; - } + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&td, 1000*timeout); sem_wait(&sem); // Make sure timer thread is killed before sem goes // out of scope - (void)pthread_cancel(pt); - (void)pthread_join(pt, &res); + hrt_work_cancel(&_hpwork); } else { @@ -283,7 +274,6 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) } } -cleanup: sem_destroy(&sem); return count; diff --git a/src/platforms/posix/px4_layer/hrt_work.h b/src/platforms/posix/include/hrt_work.h similarity index 95% rename from src/platforms/posix/px4_layer/hrt_work.h rename to src/platforms/posix/include/hrt_work.h index d926a6d250..39e53f95d2 100644 --- a/src/platforms/posix/px4_layer/hrt_work.h +++ b/src/platforms/posix/include/hrt_work.h @@ -43,12 +43,9 @@ extern sem_t _hrt_work_lock; extern struct wqueue_s g_hrt_work; void hrt_work_queue_init(void); -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); void hrt_work_cancel(struct work_s *work); -//inline void hrt_work_lock(void); -//inline void hrt_work_unlock(void); - static inline void hrt_work_lock() { //PX4_INFO("hrt_work_lock"); diff --git a/src/platforms/qurt/px4_layer/hrt_work.h b/src/platforms/qurt/include/hrt_work.h similarity index 99% rename from src/platforms/qurt/px4_layer/hrt_work.h rename to src/platforms/qurt/include/hrt_work.h index 566684eb86..92b079ac6b 100644 --- a/src/platforms/qurt/px4_layer/hrt_work.h +++ b/src/platforms/qurt/include/hrt_work.h @@ -43,7 +43,7 @@ extern sem_t _hrt_work_lock; extern struct wqueue_s g_hrt_work; void hrt_work_queue_init(void); -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); void hrt_work_cancel(struct work_s *work); inline void hrt_work_lock(void); From cc3b4b3c358d56382b9454a9d4225e0e3c0c94d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 06:59:54 +0200 Subject: [PATCH 194/493] commander: Fix param meta data --- src/modules/commander/commander_params.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index bc8f833aef..6e8e22b5cd 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -150,11 +150,12 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); /** * Engine Failure Current/Throttle Threshold * - * Engine failure triggers only below this current/throttle value + * Engine failure triggers only below this current value * * @group Commander * @min 0.0 - * @max 7.0 + * @max 30.0 + * @unit ampere */ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); @@ -167,7 +168,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @group Commander * @unit second * @min 0.0 - * @max 7.0 + * @max 60.0 */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); From f48ed934691305a748351258b209408b7b1e3c0a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:00:05 +0200 Subject: [PATCH 195/493] EKF: Fix param meta data --- .../ekf_att_pos_estimator_params.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 4702ec6cdb..357cc4c667 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -143,8 +143,8 @@ PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f); * * Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7 * - * @min 0.05 - * @max 5.0 + * @min 0.2 + * @max 3.0 * @group Position Estimator */ PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.3f); @@ -165,8 +165,8 @@ PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f); * * Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0 * - * @min 0.1 - * @max 10.0 + * @min 0.5 + * @max 3.0 * @group Position Estimator */ PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f); @@ -176,8 +176,8 @@ PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f); * * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 * - * @min 0.1 - * @max 10.0 + * @min 0.01 + * @max 1.0 * @group Position Estimator */ PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f); From 0a9e2b3923bd85477ff27c7cb7c1179a136e8d56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:00:17 +0200 Subject: [PATCH 196/493] MAVLink app: Fix param meta data --- src/modules/mavlink/mavlink.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 30c2d2b956..460e84c235 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -59,15 +59,18 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); * MAVLink component ID * @group MAVLink * @min 1 - * @max 50 + * @max 250 */ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); /** - * MAVLink type + * MAVLink airframe type + * + * + * @min 0 * @group MAVLink */ -PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); +PARAM_DEFINE_INT32(MAV_TYPE, 1); /** * Use/Accept HIL GPS message (even if not in HIL mode) From 0271a56487486c96b10a8ceb4451c0d9c0da2c55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:00:30 +0200 Subject: [PATCH 197/493] navigator: Fix param meta data --- src/modules/navigator/datalinkloss_params.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 9abc012cf2..6c2f04d7e2 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * 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 @@ -64,7 +64,8 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * Latitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -90 + * @max 90 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); @@ -75,7 +76,8 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); * Longitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -180 + * @max 180 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); @@ -86,13 +88,14 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); * Altitude of comms hold waypoint * * @unit m - * @min 0.0 + * @min -50 + * @max 30000 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); /** - * Aifield hole wait time + * Airfield hole wait time * * The amount of time in seconds the system should wait at the airfield home waypoint * From 97e3c379ab395ae5bb73c34a29cb52bed8d8b85d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:00:41 +0200 Subject: [PATCH 198/493] sensors: Fix param meta data --- src/modules/sensors/sensor_params.c | 863 +++++++++++++++++++++++++++- 1 file changed, 859 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 72d139b113..8940f0b271 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -680,6 +680,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user * to fine tune the board offset in the event of misalignment. * + * @unit radians * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); @@ -690,6 +691,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user * to fine tune the board offset in the event of misalignment. * + * @unit radians * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); @@ -700,6 +702,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user * to fine tune the board offset in the event of misalignment. * + * @unit radians * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); @@ -736,6 +739,7 @@ PARAM_DEFINE_INT32(SENS_EXT_MAG, 0); * * @min 800.0 * @max 1500.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); @@ -747,6 +751,7 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); * * @min 800.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); @@ -758,6 +763,7 @@ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); * * @min 1500.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); @@ -780,6 +786,7 @@ PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); * * @min 0.0 * @max 100.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); @@ -787,10 +794,11 @@ PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); /** * RC Channel 2 Minimum * - * Minimum value for RC channel 2 + * Minimum value for this channel. * * @min 800.0 * @max 1500.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); @@ -798,10 +806,11 @@ PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); /** * RC Channel 2 Trim * - * Mid point value (same as min for throttle) + * Mid point value (has to be set to the same as min for throttle channel). * * @min 800.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); @@ -809,10 +818,11 @@ PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); /** * RC Channel 2 Maximum * - * Maximum value for RC channel 2 + * Maximum value for this channel. * * @min 1500.0 * @max 2200.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); @@ -835,107 +845,946 @@ PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); * * @min 0.0 * @max 100.0 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); +/** + * RC Channel 3 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC3_MIN, 1000); + +/** + * RC Channel 3 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); + +/** + * RC Channel 3 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC3_MAX, 2000); + +/** + * RC Channel 3 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); +/** + * RC Channel 3 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); +/** + * RC Channel 4 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC4_MIN, 1000); + +/** + * RC Channel 4 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); + +/** + * RC Channel 4 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC4_MAX, 2000); + +/** + * RC Channel 4 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); + +/** + * RC Channel 4 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); +/** + * RC Channel 5 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC5_MIN, 1000); + +/** + * RC Channel 5 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); + +/** + * RC Channel 5 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC5_MAX, 2000); + +/** + * RC Channel 5 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); + +/** + * RC Channel 5 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); +/** + * RC Channel 6 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC6_MIN, 1000); + +/** + * RC Channel 6 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); + +/** + * RC Channel 6 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC6_MAX, 2000); + +/** + * RC Channel 6 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); + +/** + * RC Channel 6 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); +/** + * RC Channel 7 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC7_MIN, 1000); + +/** + * RC Channel 7 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); + +/** + * RC Channel 7 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC7_MAX, 2000); + +/** + * RC Channel 7 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); + +/** + * RC Channel 7 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); +/** + * RC Channel 8 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC8_MIN, 1000); + +/** + * RC Channel 8 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); + +/** + * RC Channel 8 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC8_MAX, 2000); + +/** + * RC Channel 8 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); + +/** + * RC Channel 8 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); +/** + * RC Channel 9 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC9_MIN, 1000); + +/** + * RC Channel 9 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); + +/** + * RC Channel 9 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC9_MAX, 2000); + +/** + * RC Channel 9 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); + +/** + * RC Channel 9 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); +/** + * RC Channel 10 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC10_MIN, 1000); + +/** + * RC Channel 10 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); + +/** + * RC Channel 10 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC10_MAX, 2000); + +/** + * RC Channel 10 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); + +/** + * RC Channel 10 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); +/** + * RC Channel 11 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC11_MIN, 1000); + +/** + * RC Channel 11 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); + +/** + * RC Channel 11 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC11_MAX, 2000); + +/** + * RC Channel 11 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); + +/** + * RC Channel 11 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); +/** + * RC Channel 12 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC12_MIN, 1000); + +/** + * RC Channel 12 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); + +/** + * RC Channel 12 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC12_MAX, 2000); + +/** + * RC Channel 12 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); + +/** + * RC Channel 12 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); +/** + * RC Channel 13 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC13_MIN, 1000); + +/** + * RC Channel 13 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); + +/** + * RC Channel 13 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC13_MAX, 2000); + +/** + * RC Channel 13 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); + +/** + * RC Channel 13 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); +/** + * RC Channel 14 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC14_MIN, 1000); + +/** + * RC Channel 14 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); + +/** + * RC Channel 14 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); + +/** + * RC Channel 14 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); + +/** + * RC Channel 14 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +/** + * RC Channel 15 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_MIN, 1000); + +/** + * RC Channel 15 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); + +/** + * RC Channel 15 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_MAX, 2000); + +/** + * RC Channel 15 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); + +/** + * RC Channel 15 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); +/** + * RC Channel 16 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_MIN, 1000); + +/** + * RC Channel 16 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_TRIM, 1500); + +/** + * RC Channel 16 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_MAX, 2000); + +/** + * RC Channel 16 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_REV, 1.0f); + +/** + * RC Channel 16 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f); +/** + * RC Channel 17 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_MIN, 1000); + +/** + * RC Channel 17 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_TRIM, 1500); + +/** + * RC Channel 17 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_MAX, 2000); + +/** + * RC Channel 17 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_REV, 1.0f); + +/** + * RC Channel 17 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f); +/** + * RC Channel 18 Minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_MIN, 1000); + +/** + * RC Channel 18 Trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_TRIM, 1500); + +/** + * RC Channel 18 Maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_MAX, 2000); + +/** + * RC Channel 18 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); + +/** + * RC Channel 18 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +/** + * Enable relay control of relay 1 mapped to the Spektrum receiver power supply + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif @@ -952,6 +1801,8 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /** * Scaling factor for battery voltage sensor on PX4IO. * + * @min 1 + * @max 100000 * @group Battery Calibration */ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); @@ -1231,8 +2082,12 @@ PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0); /** * Failsafe channel PWM threshold. * - * @min 800 + * Set to a value slightly above the PWM value assumed by throttle in a failsafe event, + * but ensure it is below the PWM value assumed by throttle during normal operation. + * + * @min 0 * @max 2200 + * @unit us * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); From 77ff09792e349c1971330c59e97490de48db318c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:00:54 +0200 Subject: [PATCH 199/493] vtol: Fix param meta data --- .../vtol_att_control_params.c | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) 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 f302314a23..33c095036c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -43,10 +43,10 @@ /** * VTOL number of engines * - * @min 1 + * @min 0 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_MOT_COUNT,0); +PARAM_DEFINE_INT32(VT_MOT_COUNT, 0); /** * Idle speed of VTOL when in multicopter mode @@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(VT_MOT_COUNT,0); * @min 900 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); +PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900); /** * Minimum airspeed in multicopter mode @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN, 10.0f); /** * Maximum airspeed in multicopter mode @@ -74,7 +74,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX, 30.0f); /** * Trim airspeed when in multicopter mode @@ -84,7 +84,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM, 10.0f); /** * Permanent stabilization in fw mode @@ -96,7 +96,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); +PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0); /** * Fixed wing pitch trim @@ -107,7 +107,7 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); +PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM, 0.0f); /** * Motor max power @@ -118,18 +118,18 @@ PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); * @min 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f); +PARAM_DEFINE_FLOAT(VT_POWER_MAX, 120.0f); /** * Propeller efficiency parameter * * Influences propeller efficiency at different power settings. Should be tuned beforehand. * - * @min 0.5 + * @min 0.0 * @max 0.9 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); +PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); /** * Total airspeed estimate low-pass filter gain @@ -140,7 +140,7 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); * @max 0.99 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); +PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); /** * VTOL Type (Tailsitter=0, Tiltrotor=1) @@ -160,4 +160,4 @@ PARAM_DEFINE_INT32(VT_TYPE, 0); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0); +PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0); From abbbfdfceeabe9eb0edfdc16cf556ff9fb69b958 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:10:13 +0200 Subject: [PATCH 200/493] mc pos control: Fix params and descriptions --- .../mc_pos_control/mc_pos_control_params.c | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) 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 a09ed4a3e6..501cd695b8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +35,7 @@ * @file mc_pos_control_params.c * Multicopter position controller parameters. * - * @author Anton Babushkin + * @author Anton Babushkin */ #include @@ -107,7 +106,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); * * @unit m/s * @min 0.0 - * @max 8 m/s + * @max 8.0 * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); @@ -184,7 +183,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); * * Limits maximum tilt in AUTO and POSCTRL modes during flight. * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -196,7 +195,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); * * Limits maximum tilt angle on landing. * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -215,7 +214,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); /** * Max manual roll * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -225,7 +224,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX, 35.0f); /** * Max manual pitch * - * @unit deg + * @unit degree * @min 0.0 * @max 90.0 * @group Multicopter Position Control @@ -235,7 +234,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f); /** * Max manual yaw rate * - * @unit deg/s + * @unit degree / s * @min 0.0 * @group Multicopter Position Control */ From 395ef5562c2733268657ac50ff6ee1ace951e7fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:10:26 +0200 Subject: [PATCH 201/493] navigator: Fix param meta data and comments --- src/modules/navigator/datalinkloss_params.c | 10 ++++---- src/modules/navigator/navigator_params.c | 26 ++++++++++++--------- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 6c2f04d7e2..e1336214e3 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -36,7 +36,7 @@ * * Parameters for DLL * - * @author Thomas Gubler + * @author Thomas Gubler */ #include @@ -64,8 +64,8 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * Latitude of comms hold waypoint * * @unit degrees * 1e7 - * @min -90 - * @max 90 + * @min -900000000 + * @max 900000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); @@ -76,8 +76,8 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); * Longitude of comms hold waypoint * * @unit degrees * 1e7 - * @min -180 - * @max 180 + * @min -1800000000 + * @max 1800000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ef4a8dc0c7..90384d85af 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * * Parameters for navigator in general * - * @author Julian Oes - * @author Thomas Gubler + * @author Julian Oes + * @author Thomas Gubler */ #include @@ -49,9 +49,9 @@ * * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). * - * @unit meters - * @min 20 - * @max 200 + * @unit meter + * @min 25 + * @max 1000 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); @@ -61,9 +61,9 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * * Default acceptance radius, overridden by acceptance radius of waypoint if set. * - * @unit meters + * @unit meter * @min 0.05 - * @max 200 + * @max 200.0 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); @@ -74,6 +74,7 @@ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules * * @min 0 + * @max 1 * @group Mission */ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); @@ -84,6 +85,7 @@ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules * * @min 0 + * @max 1 * @group Mission */ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); @@ -94,7 +96,8 @@ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); * Latitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -900000000 + * @max 900000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); @@ -105,7 +108,8 @@ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); * Longitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0 + * @min -1800000000 + * @max 1800000000 * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); @@ -116,7 +120,7 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); * Altitude of airfield home waypoint * * @unit m - * @min 0.0 + * @min -50 * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); From 7b05165249fb47bb8ff5d213f0b9e43940def663 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:15:23 +0200 Subject: [PATCH 202/493] Param unit test: Fix CLANG compile warning --- unittests/param_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp index 44ae4df068..bda49ae86d 100644 --- a/unittests/param_test.cpp +++ b/unittests/param_test.cpp @@ -52,8 +52,8 @@ void _assert_parameter_int_value(param_t param, int32_t expected) { int32_t value; int result = param_get(param, &value); - ASSERT_EQ(0, result) << printf("param_get (%i) did not return parameter\n", param); - ASSERT_EQ(expected, value) << printf("value for param (%i) doesn't match default value\n", param); + ASSERT_EQ(0, result) << printf("param_get (%lu) did not return parameter\n", param); + ASSERT_EQ(expected, value) << printf("value for param (%lu) doesn't match default value\n", param); } void _set_all_int_parameters_to(int32_t value) From 5982eaaf34f53accbf8040c81e5c0ece3ed6487b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 10:54:09 +0200 Subject: [PATCH 203/493] MC pos control: Enforce minimum throttle in manual attitude control mode only if not landed, else default to idle throttle --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ad1ba56652..54c7608632 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1414,7 +1414,11 @@ MulticopterPositionControl::task_main() /* control throttle directly if no climb rate controller is active */ if (!_control_mode.flag_control_climb_rate_enabled) { _att_sp.thrust = math::min(_manual.z, _params.thr_max); - _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); + + /* enforce minimum throttle if not landed */ + if (!_vehicle_status.condition_landed) { + _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); + } } /* construct attitude setpoint rotation matrix */ From 9a3658836154bea32f0f02b3695fd3deddbcbadb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:15:27 +0200 Subject: [PATCH 204/493] MC land detector: If no position information is available, rely on the armed state exclusively to infer the landed condition. --- src/modules/land_detector/MulticopterLandDetector.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 1490232a4f..5391f7769a 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -100,6 +100,14 @@ bool MulticopterLandDetector::update() return true; } + // return status based on armed state if no position lock is available + if (_vehicleGlobalPosition.timestamp == 0 || + hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { + + // no position lock - not landed if armed + return !_arming.armed; + } + const uint64_t now = hrt_absolute_time(); // check if we are moving vertically From 5549d480fd55817262cb70ebff299a641285c2c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:16:17 +0200 Subject: [PATCH 205/493] MC land detector: Update params and fix docs. Allow more motion during the landed state, but become more picky on throttle. --- .../land_detector/land_detector_params.c | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index f182495ac3..56f971dd35 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -43,29 +43,29 @@ /** * Multicopter max climb rate * - * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * Maximum vertical velocity allowed in the landed state (m/s up and down) * * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 1.00f); /** * Multicopter max horizontal velocity * - * Maximum horizontal velocity allowed to trigger a land (m/s) + * Maximum horizontal velocity allowed in the landed state (m/s) * * @unit m/s * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.50f); /** * Multicopter max rotation * - * Maximum allowed around each axis to trigger a land (degrees per second) + * Maximum allowed around each axis allowed in the landed state (degrees per second) * * @unit deg/s * @@ -76,19 +76,19 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); /** * Multicopter max throttle * - * Maximum actuator output on throttle before triggering a land + * Maximum actuator output on throttle allowed in the landed state * * @min 0.1 * @max 0.5 * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f); /** * Fixedwing max horizontal velocity * - * Maximum horizontal velocity allowed to trigger a land (m/s) + * Maximum horizontal velocity allowed in the landed state (m/s) * * @min 0.5 * @max 10 @@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); /** * Fixedwing max climb rate * - * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * Maximum vertical velocity allowed in the landed state (m/s up and down) * * @min 5 * @max 20 @@ -114,7 +114,7 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); /** * Airspeed max * - * Maximum airspeed allowed to trigger a land (m/s) + * Maximum airspeed allowed in the landed state (m/s) * * @min 4 * @max 20 From 5bec38b37dbdf87720b98021850141e817de4191 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:17:39 +0200 Subject: [PATCH 206/493] MC land detector: Slightly decrease allowed vertical motion during landed state. This is important so that fast descends do not result in a false positive landed state --- src/modules/land_detector/land_detector_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 56f971dd35..77bac2ad74 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -49,7 +49,7 @@ * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.60f); /** * Multicopter max horizontal velocity From c28a69fba8873b7551f1031e32f480c4f9a522ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:36:59 +0200 Subject: [PATCH 207/493] Mixer test: Ensure its not susceptible to timing jitter of the test harness --- src/systemcmds/tests/test_mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index e9500d2d16..d0eb1eb1ff 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -200,7 +200,7 @@ int test_mixer(int argc, char *argv[]) hrt_abstime starttime = hrt_absolute_time(); unsigned sleepcount = 0; - while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { + while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) { /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); From ece87a3fa2afd4e6aa1ab7c85fd15fa42ba05515 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 07:33:52 +0200 Subject: [PATCH 208/493] Mixer test: Fixed compile warnings --- src/systemcmds/tests/test_mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index d0eb1eb1ff..acde4a1a50 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -259,7 +259,7 @@ int test_mixer(int argc, char *argv[]) for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; - if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + if (abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; @@ -333,7 +333,7 @@ int test_mixer(int argc, char *argv[]) /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && - fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; From a33700a7ec29221a656af4a1372636e9c2f2ff95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 09:48:29 +0200 Subject: [PATCH 209/493] Actuator controls: Add indices for channels and groups --- msg/actuator_controls.msg | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 414eb06ddb..66e12325d2 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -1,5 +1,11 @@ uint8 NUM_ACTUATOR_CONTROLS = 8 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 +uint8 INDEX_THROTTLE = 3 +uint8 INDEX_FLAPS = 4 +uint8 GROUP_INDEX_ATTITUDE = 0 uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control From cde8d72694e342e37ab3ba1879f139c0aa447084 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Jun 2015 10:02:33 +0200 Subject: [PATCH 210/493] PWM output limiter: Improve comments. --- src/modules/systemlib/pwm_limit/pwm_limit.c | 17 +++++++++++------ src/modules/systemlib/pwm_limit/pwm_limit.h | 9 ++++----- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index adcfb703c0..2f72d347c6 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: Julian Oes + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,9 +34,9 @@ /** * @file pwm_limit.c * - * Lib to limit PWM output + * Library for PWM output limiting * - * @author Julian Oes + * @author Julian Oes */ #include "pwm_limit.h" @@ -46,6 +45,8 @@ #include #include +#define PROGRESS_INT_SCALING 10000 + void pwm_limit_init(pwm_limit_t *limit) { limit->state = PWM_LIMIT_STATE_INIT; @@ -112,7 +113,11 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ { hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); - progress = diff * 10000 / RAMP_TIME_US; + progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; + + if (progress > PROGRESS_INT_SCALING) { + progress = PROGRESS_INT_SCALING; + } for (unsigned i=0; i + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,11 +32,11 @@ ****************************************************************************/ /** - * @file pwm_limit.h + * @file pwm_limit.c * - * Lib to limit PWM output + * Library for PWM output limiting * - * @author Julian Oes + * @author Julian Oes */ #ifndef PWM_LIMIT_H_ From 1b4405ee3ac2afe328695cfd28449b1cbe24a606 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 09:49:33 +0200 Subject: [PATCH 211/493] FMU driver: Set throttle to zero if in PWM ramp mode --- src/drivers/px4fmu/fmu.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b340694bf0..2047046b9b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -155,7 +155,7 @@ private: pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; - pwm_limit_t _pwm_limit; + static pwm_limit_t _pwm_limit; uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; @@ -241,6 +241,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +pwm_limit_t PX4FMU::_pwm_limit; namespace { @@ -272,7 +273,6 @@ PX4FMU::PX4FMU() : _control_subs{-1}, _actuator_output_topic_instance(-1), _poll_fds_num(0), - _pwm_limit{}, _failsafe_pwm{0}, _disarmed_pwm{0}, _reverse_pwm_mask(0), @@ -827,6 +827,18 @@ PX4FMU::control_callback(uintptr_t handle, const actuator_controls_s *controls = (actuator_controls_s *)handle; input = controls[control_group].control[control_index]; + + /* motor spinup phase - lock throttle to zero */ + if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + */ + input = 0.0f; + } + } + return 0; } From 6697ffb668ecf74448417d3ca410f97f10276161 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 09:49:52 +0200 Subject: [PATCH 212/493] IO driver: Set throttle to zero if in PWM ramp mode --- src/modules/px4iofirmware/mixer.cpp | 44 +++++++++++++++++++---------- 1 file changed, 29 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 5e6a3a585a..b5d93daea7 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -49,6 +49,7 @@ #include #include +#include extern "C" { //#define DEBUG @@ -318,13 +319,6 @@ mixer_callback(uintptr_t handle, case MIX_OVERRIDE: if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); - if (control_group == 0 && control_index == 0) { - control += REG_TO_FLOAT(r_setup_trim_roll); - } else if (control_group == 0 && control_index == 1) { - control += REG_TO_FLOAT(r_setup_trim_pitch); - } else if (control_group == 0 && control_index == 2) { - control += REG_TO_FLOAT(r_setup_trim_yaw); - } break; } return -1; @@ -333,13 +327,6 @@ mixer_callback(uintptr_t handle, /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); - if (control_group == 0 && control_index == 0) { - control += REG_TO_FLOAT(r_setup_trim_roll); - } else if (control_group == 0 && control_index == 1) { - control += REG_TO_FLOAT(r_setup_trim_pitch); - } else if (control_group == 0 && control_index == 2) { - control += REG_TO_FLOAT(r_setup_trim_yaw); - } break; } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); @@ -353,6 +340,33 @@ mixer_callback(uintptr_t handle, return -1; } + /* apply trim offsets for override channels */ + if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_ROLL) { + control += REG_TO_FLOAT(r_setup_trim_roll); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_PITCH) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_YAW) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } + } + + /* motor spinup phase - lock throttle to zero */ + if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + */ + control = 0.0f; + } + } + /* limit output */ if (control > 1.0f) { control = 1.0f; From 319f9d820f2c77cb8b63cd3342accbd5a183d6de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 12:55:28 +0200 Subject: [PATCH 213/493] telemetry: Crank up rates to make param downloads and other things less painful --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++---- ROMFS/px4fmu_test/mixers/IO_pass.mix | 24 ++++++++++++++++++++++++ 2 files changed, 28 insertions(+), 4 deletions(-) create mode 100644 ROMFS/px4fmu_test/mixers/IO_pass.mix diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0fb6e117cc..54a469aff8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -457,13 +457,13 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_F "-r 1000 -d /dev/ttyS0" + set MAVLINK_F "-r 5000 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - set MAVLINK_F "-r 1000" + set MAVLINK_F "-r 5000" fi fi @@ -479,11 +479,11 @@ then # but this works for now if param compare SYS_COMPANION 921600 then - mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 -x + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 -x + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 60000 -x fi if param compare SYS_COMPANION 157600 then diff --git a/ROMFS/px4fmu_test/mixers/IO_pass.mix b/ROMFS/px4fmu_test/mixers/IO_pass.mix new file mode 100644 index 0000000000..42321f0ad9 --- /dev/null +++ b/ROMFS/px4fmu_test/mixers/IO_pass.mix @@ -0,0 +1,24 @@ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 963972721dc9d709e18f55c2c4adb4e6f141f115 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 13:21:09 +0200 Subject: [PATCH 214/493] MAVLink app: Support rudimentary radio config. --- src/modules/mavlink/mavlink.c | 16 +++++++++ src/modules/mavlink/mavlink_main.cpp | 49 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 2 ++ 3 files changed, 67 insertions(+) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 460e84c235..62d40d4b74 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,10 +64,26 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); /** +<<<<<<< Updated upstream * MAVLink airframe type * * * @min 0 +======= + * MAVLink Radio ID + * + * When non-zero the MAVLink app will attempt to configure the + * radio to this ID and re-set the parameter to 0. + * + * @group MAVLink + * @min 0 + * @max 240 + */ +PARAM_DEFINE_INT32(MAV_RADIO_ID, 0); + +/** + * MAVLink type +>>>>>>> Stashed changes * @group MAVLink */ PARAM_DEFINE_INT32(MAV_TYPE, 1); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 872406775b..3866e86de6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -137,6 +137,7 @@ Mavlink::Mavlink() : _mavlink_ftp(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), + _radio_id(0), _logbuffer {}, _total_counter(0), _receive_thread {}, @@ -170,6 +171,7 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), + _param_radio_id(0), _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), _param_forward_externalsp(0), @@ -489,6 +491,7 @@ void Mavlink::mavlink_update_system(void) if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); + _param_radio_id = param_find("MAV_RADIO_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); _param_forward_externalsp = param_find("MAV_FWDEXTSP"); @@ -504,6 +507,7 @@ void Mavlink::mavlink_update_system(void) int32_t component_id; param_get(_param_component_id, &component_id); + param_get(_param_radio_id, &_radio_id); /* only allow system ID and component ID updates * after reboot - not during operation */ @@ -1508,6 +1512,51 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); } + /* radio config check */ + if (_radio_id != 0 && _rstatus.type == TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { + /* request to configure radio and radio is present */ + FILE *fs = fdopen(_uart_fd, "w"); + + if (fs) { + /* switch to AT command mode */ + usleep(1200000); + fprintf(fs, "+++\n"); + usleep(1200000); + + if (_radio_id > 0) { + /* set channel */ + fprintf(fs, "ATS3=%u\n", _radio_id); + usleep(200000); + } else { + /* reset to factory defaults */ + fprintf(fs, "AT&F\n"); + usleep(200000); + } + + /* write config */ + fprintf(fs, "AT&W"); + usleep(200000); + + /* reboot */ + fprintf(fs, "ATZ"); + usleep(200000); + + warnx("configured radio"); + // XXX NuttX suffers from a bug where + // fclose() also closes the fd, not just + // the file stream. Since this is a one-time + // config thing, we leave the file struct + // allocated. + //fclose(fs); + } else { + warnx("opening %d as file failed", _uart_fd); + } + + /* reset param and save */ + _radio_id = 0; + param_set(_param_radio_id, &_radio_id); + } + if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index cf95c4d40c..777836f688 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -324,6 +324,7 @@ private: MAVLINK_MODE _mode; mavlink_channel_t _channel; + int32_t _radio_id; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; @@ -381,6 +382,7 @@ private: bool _param_initialized; param_t _param_system_id; param_t _param_component_id; + param_t _param_radio_id; param_t _param_system_type; param_t _param_use_hil_gps; param_t _param_forward_externalsp; From f0e9817f2b2cd2eef15af1c4b6c00cc94fd5d39e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 15:19:57 +0200 Subject: [PATCH 215/493] ROMFS: Adjust onboard data rate --- 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 54a469aff8..0a4887a639 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -483,7 +483,7 @@ then fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 60000 -x + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 5000 -x fi if param compare SYS_COMPANION 157600 then From b8609f99d746bf8926b2ad2fe6dbab95588223c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 15:24:05 +0200 Subject: [PATCH 216/493] MAVLink app: Fix parameter comments --- src/modules/mavlink/mavlink.c | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 62d40d4b74..b1d369cbf4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -33,9 +33,9 @@ /** * @file mavlink.c - * Adapter functions expected by the protocol library + * Define MAVLink specific parameters * - * @author Lorenz Meier + * @author Lorenz Meier */ #include @@ -46,7 +46,6 @@ #include "mavlink_bridge_header.h" #include -/* define MAVLink specific parameters */ /** * MAVLink system ID * @group MAVLink @@ -64,34 +63,31 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); /** -<<<<<<< Updated upstream - * MAVLink airframe type - * - * - * @min 0 -======= * MAVLink Radio ID * * When non-zero the MAVLink app will attempt to configure the - * radio to this ID and re-set the parameter to 0. + * radio to this ID and re-set the parameter to 0. If the value + * is negative it will reset the complete radio config to + * factory defaults. * * @group MAVLink - * @min 0 + * @min -1 * @max 240 */ PARAM_DEFINE_INT32(MAV_RADIO_ID, 0); /** - * MAVLink type ->>>>>>> Stashed changes + * MAVLink airframe type + * + * @min 1 * @group MAVLink */ PARAM_DEFINE_INT32(MAV_TYPE, 1); /** - * Use/Accept HIL GPS message (even if not in HIL mode) + * Use/Accept HIL GPS message even if not in HIL mode * - * If set to 1 incomming HIL GPS messages are parsed. + * If set to 1 incoming HIL GPS messages are parsed. * * @group MAVLink */ @@ -100,8 +96,8 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); /** * Forward external setpoint messages * - * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard - * control mode + * If set to 1 incoming external setpoint messages will be directly forwarded + * to the controllers if in offboard control mode * * @group MAVLink */ From 3ef62121556efeb141b6e3fc36095735aec68654 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Jun 2015 15:26:05 +0200 Subject: [PATCH 217/493] MAVLink app: Less verbose during radio config --- src/modules/mavlink/mavlink_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3866e86de6..1f43031be2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1541,7 +1541,6 @@ Mavlink::task_main(int argc, char *argv[]) fprintf(fs, "ATZ"); usleep(200000); - warnx("configured radio"); // XXX NuttX suffers from a bug where // fclose() also closes the fd, not just // the file stream. Since this is a one-time @@ -1549,7 +1548,7 @@ Mavlink::task_main(int argc, char *argv[]) // allocated. //fclose(fs); } else { - warnx("opening %d as file failed", _uart_fd); + warnx("open fd %d failed", _uart_fd); } /* reset param and save */ From 93dfc435a4b40f7b8eac541d902657ec881ff033 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 17:53:19 +0200 Subject: [PATCH 218/493] change the nested if structure which tries all i2c busses to a loop. --- src/drivers/px4flow/px4flow.cpp | 81 ++++++++++++++++++--------------- 1 file changed, 45 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 0704f16c91..ba438e276e 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -656,65 +656,74 @@ start() errx(1, "already started"); } - /* create the driver */ - g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - + const int busses_to_try[] = { + PX4_I2C_BUS_EXPANSION, #ifdef PX4_I2C_BUS_ESC - delete g_dev; - /* try 2nd bus */ - g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); + PX4_I2C_BUS_ESC, + #endif + PX4_I2C_BUS_ONBOARD, + -1 + }; + const int *cur_bus = busses_to_try; + while(*cur_bus != -1) { + /* create the driver */ + //warnx("trying bus %d", *cur_bus); + g_dev = new PX4FLOW(*cur_bus); if (g_dev == nullptr) { - goto fail; + /* this is a fatal error */ + break; } - - if (OK != g_dev->init()) { - #endif - - delete g_dev; - /* try 3rd bus */ - g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - #ifdef PX4_I2C_BUS_ESC + + /* init the driver: */ + if (OK == g_dev->init()) { + /* success! */ + break; } - #endif + + /* destroy it again because it failed. */ + delete g_dev; + g_dev = nullptr; + + /* try next! */ + cur_bus++; } - + + /* check whether we found it: */ + if (*cur_bus == -1) { + goto not_found; + } + + /* check for failure: */ + if (g_dev == nullptr) { + goto fatal_fail; + } + /* set the poll rate to default, starts automatic data collection */ fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - goto fail; + goto fatal_fail; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - goto fail; + goto fatal_fail; } exit(0); -fail: +not_found: + /* for now we do the same as if there was a fatal failure. */ + warnx("PX4FLOW not found on I2C busses"); + +fatal_fail: if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - errx(1, "no PX4FLOW connected over I2C"); + errx(1, "PX4FLOW could not be started over I2C"); } /** From 641fd26877ce50f52bc6d5f35809d325c92c89c5 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 30 Jun 2015 09:10:06 -0700 Subject: [PATCH 219/493] QuRT: Fixed PX4_ISFINITE QuRT needs to use the builtin version of isfinite so for the qurt build PX4_ISFINITE(x) is defined as __builtin_isfinite(x). Signed-off-by: Mark Charlebois --- src/platforms/px4_defines.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index d2cc7c2439..f85baa3b75 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -225,7 +225,7 @@ __END_DECLS #define SIOCDEVPRIVATE 999999 // Missing math.h defines -#define PX4_ISFINITE(x) isfinite(x) +#define PX4_ISFINITE(x) __builtin_isfinite(x) // FIXME - these are missing for clang++ but not for clang #if defined(__cplusplus) From 34d15fe63191873e3293bb7ba41cacb7172ba76b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 30 Jun 2015 09:23:37 -0700 Subject: [PATCH 220/493] Gyrosim cleanup Removed unused code. Reset reschedule interval for sampling when the sampling rate is changed. The rate is always 1000Hz as it is set to the default value. Signed-off-by: Mark Charlebois --- .../posix/drivers/gyrosim/gyrosim.cpp | 114 ++++++------------ 1 file changed, 37 insertions(+), 77 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index fa7f92abba..f2dd548679 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -41,6 +41,9 @@ * @author Mark Charlebois */ +#define __STDC_FORMAT_MACROS +#include + #include #include @@ -85,37 +88,19 @@ #define MPUREG_CONFIG 0x1A #define MPUREG_GYRO_CONFIG 0x1B #define MPUREG_ACCEL_CONFIG 0x1C -#define MPUREG_INT_PIN_CFG 0x37 -#define MPUREG_INT_ENABLE 0x38 #define MPUREG_INT_STATUS 0x3A -#define MPUREG_USER_CTRL 0x6A -#define MPUREG_PWR_MGMT_1 0x6B -#define MPUREG_PWR_MGMT_2 0x6C #define MPUREG_PRODUCT_ID 0x0C // Product ID Description for GYROSIM // high 4 bits low 4 bits // Product Name Product Revision #define GYROSIMES_REV_C4 0x14 -#define GYROSIMES_REV_C5 0x15 -#define GYROSIMES_REV_D6 0x16 -#define GYROSIMES_REV_D7 0x17 -#define GYROSIMES_REV_D8 0x18 -#define GYROSIM_REV_C4 0x54 -#define GYROSIM_REV_C5 0x55 -#define GYROSIM_REV_D6 0x56 -#define GYROSIM_REV_D7 0x57 -#define GYROSIM_REV_D8 0x58 -#define GYROSIM_REV_D9 0x59 -#define GYROSIM_REV_D10 0x5A -#define GYROSIM_ACCEL_DEFAULT_RATE 1000 -#define GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define GYROSIM_ACCEL_DEFAULT_RATE 1000 -#define GYROSIM_GYRO_DEFAULT_RATE 1000 -#define GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 +#define GYROSIM_GYRO_DEFAULT_RATE 1000 -#define GYROSIM_ONE_G 9.80665f +#define GYROSIM_ONE_G 9.80665f #ifdef PX4_SPI_BUS_EXT #define EXTERNAL_BUS PX4_SPI_BUS_EXT @@ -186,16 +171,6 @@ private: perf_counter_t _system_latency_perf; perf_counter_t _controller_latency_perf; - uint8_t _register_wait; - uint64_t _reset_wait; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - math::LowPassFilter2p _gyro_filter_x; - math::LowPassFilter2p _gyro_filter_y; - math::LowPassFilter2p _gyro_filter_z; - enum Rotation _rotation; // last temperature reading for print_info() @@ -372,14 +347,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")), _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), - _register_wait(0), - _reset_wait(0), - _accel_filter_x(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_y(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_x(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_y(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_z(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _last_temperature(0) { @@ -571,6 +538,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) void GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) { + PX4_INFO("GYROSIM::_set_sample_rate %uHz", desired_sample_rate_hz); if (desired_sample_rate_hz == 0 || desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { @@ -580,8 +548,16 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) uint8_t div = 1000 / desired_sample_rate_hz; if(div>200) div=200; if(div<1) div=1; + + // This does nothing in the simulator but writes the value in the "register" so + // register dumps look correct write_reg(MPUREG_SMPLRT_DIV, div-1); + _sample_rate = 1000 / div; + PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); + _call_interval = 1000000/_sample_rate; + hrt_cancel(&_call); + hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); } ssize_t @@ -775,9 +751,6 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; @@ -785,22 +758,11 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - - - float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; + _call_interval = ticks; + + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); /* if we need to start the poll state machine, do it */ if (want_start) @@ -839,14 +801,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) _set_sample_rate(arg); return OK; - case ACCELIOCGLOWPASS: - return _accel_filter_x.get_cutoff_freq(); - case ACCELIOCSLOWPASS: - // set software filtering - _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case ACCELIOCSSCALE: @@ -915,13 +870,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) _set_sample_rate(arg); return OK; - case GYROIOCGLOWPASS: - return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: - // set hardware filtering - _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case GYROIOCSSCALE: @@ -981,9 +930,6 @@ GYROSIM::set_accel_range(unsigned max_g_in) // workaround for bugged versions of MPU6k (rev C) switch (_product) { case GYROSIMES_REV_C4: - case GYROSIMES_REV_C5: - case GYROSIM_REV_C4: - case GYROSIM_REV_C5: write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); _accel_range_scale = (GYROSIM_ONE_G / 4096.0f); _accel_range_m_s2 = 8.0f * GYROSIM_ONE_G; @@ -1030,7 +976,9 @@ GYROSIM::start() _gyro_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + if (_call_interval > 0) { + hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + } } void @@ -1051,6 +999,18 @@ GYROSIM::measure_trampoline(void *arg) void GYROSIM::measure() { + static int x = 0; + +#if 0 + // Verify the samples are being taken at the expected rate + if (x == 99) { + x = 0; + PX4_INFO("GYROSIM::measure %" PRIu64, hrt_absolute_time()); + } + else { + x++; + } +#endif struct MPUReport mpu_report; /* start measuring */ @@ -1071,7 +1031,7 @@ GYROSIM::measure() * Report buffers. */ accel_report arb; - gyro_report grb; + gyro_report grb; // for now use local time but this should be the timestamp of the simulator grb.timestamp = hrt_absolute_time(); @@ -1368,7 +1328,7 @@ test() } /* do a simple demand read */ - sz = read(fd, &a_report, sizeof(a_report)); + sz = px4_read(fd, &a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report)); @@ -1388,7 +1348,7 @@ test() (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ - sz = read(fd_gyro, &g_report, sizeof(g_report)); + sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report)); From 7a933483405962291eb7706c7de169ce50223955 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 18:28:19 +0200 Subject: [PATCH 221/493] implemented retrying the connection to the px4flow sensor before giving up. --- src/drivers/px4flow/px4flow.cpp | 133 ++++++++++++++++++-------------- 1 file changed, 76 insertions(+), 57 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index ba438e276e..8d0af42524 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -636,7 +636,11 @@ namespace px4flow #endif const int ERROR = -1; -PX4FLOW *g_dev; +PX4FLOW *g_dev = nullptr; +bool start_in_progress = false; + +const int START_RETRY_COUNT = 5; +const int START_RETRY_TIMEOUT = 1000; void start(); void stop(); @@ -651,78 +655,93 @@ void start() { int fd; + + /* entry check: */ + if (start_in_progress) { + errx(1, "start in progress"); + } + start_in_progress = true; if (g_dev != nullptr) { + start_in_progress = false; errx(1, "already started"); } - const int busses_to_try[] = { - PX4_I2C_BUS_EXPANSION, - #ifdef PX4_I2C_BUS_ESC - PX4_I2C_BUS_ESC, - #endif - PX4_I2C_BUS_ONBOARD, - -1 - }; + int retry_nr = 0; + while (1) { + const int busses_to_try[] = { + PX4_I2C_BUS_EXPANSION, + #ifdef PX4_I2C_BUS_ESC + PX4_I2C_BUS_ESC, + #endif + PX4_I2C_BUS_ONBOARD, + -1 + }; - const int *cur_bus = busses_to_try; - while(*cur_bus != -1) { - /* create the driver */ - //warnx("trying bus %d", *cur_bus); - g_dev = new PX4FLOW(*cur_bus); - if (g_dev == nullptr) { - /* this is a fatal error */ - break; + const int *cur_bus = busses_to_try; + + while(*cur_bus != -1) { + /* create the driver */ + /* warnx("trying bus %d", *cur_bus); */ + g_dev = new PX4FLOW(*cur_bus); + if (g_dev == nullptr) { + /* this is a fatal error */ + break; + } + + /* init the driver: */ + if (OK == g_dev->init()) { + /* success! */ + break; + } + + /* destroy it again because it failed. */ + delete g_dev; + g_dev = nullptr; + + /* try next! */ + cur_bus++; } - - /* init the driver: */ - if (OK == g_dev->init()) { + + /* check whether we found it: */ + if (*cur_bus != -1) { + + /* check for failure: */ + if (g_dev == nullptr) { + break; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + break; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { + break; + } + /* success! */ + start_in_progress = false; + exit(0); + } + + if (retry_nr < START_RETRY_COUNT) { + warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr); + usleep(START_RETRY_TIMEOUT * 1000); + retry_nr++; + } else { break; } - - /* destroy it again because it failed. */ - delete g_dev; - g_dev = nullptr; - - /* try next! */ - cur_bus++; } - /* check whether we found it: */ - if (*cur_bus == -1) { - goto not_found; - } - - /* check for failure: */ - if (g_dev == nullptr) { - goto fatal_fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fatal_fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - goto fatal_fail; - } - - exit(0); - -not_found: - /* for now we do the same as if there was a fatal failure. */ - warnx("PX4FLOW not found on I2C busses"); - -fatal_fail: - if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - + + start_in_progress = false; errx(1, "PX4FLOW could not be started over I2C"); } From 1b01c54dd1ab2681b8c571dfac08753919d8b66e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 30 Jun 2015 09:53:01 -0700 Subject: [PATCH 222/493] POSIX: fixed build error for unused variable Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index f2dd548679..0fde8b293a 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -999,9 +999,10 @@ GYROSIM::measure_trampoline(void *arg) void GYROSIM::measure() { - static int x = 0; #if 0 + static int x = 0; + // Verify the samples are being taken at the expected rate if (x == 99) { x = 0; From 14bf8bb277e76a982e23ca0d1f094724e36b2ffc Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 30 Jun 2015 12:08:42 -0700 Subject: [PATCH 223/493] POSIX: Critical fix for vdev_posix Last fix for vdev_posix.cpp introduced a sleep from within a HRT work item callback which blocks the HRT queue. The code in uORBDevices_posix.cpp that handles message throttling was commented out for posix. The code was re-enabled and now seems to work. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev_posix.cpp | 22 +++------------------- src/modules/uORB/uORBDevices_posix.cpp | 5 ----- 2 files changed, 3 insertions(+), 24 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index f9a6ebc559..86c2230526 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -54,24 +54,10 @@ using namespace device; extern "C" { -struct timerData { - sem_t &sem; - struct timespec &ts; - - timerData(sem_t &s, struct timespec &t) : sem(s), ts(t) {} - ~timerData() {} -}; - static void timer_cb(void *data) { - struct timerData *td = (struct timerData *)data; - - if (td->ts.tv_sec) { - sleep(td->ts.tv_sec); - } - usleep(td->ts.tv_nsec/1000); - sem_post(&(td->sem)); - + sem_t *p_sem = (sem_t *)data; + sem_post(p_sem); PX4_DEBUG("timer_handler: Timer expired"); } @@ -211,7 +197,6 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) int count = 0; int ret; unsigned int i; - struct timespec ts; PX4_DEBUG("Called px4_poll timeout = %d", timeout); sem_init(&sem, 0, 0); @@ -242,8 +227,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) // Use a work queue task work_s _hpwork; - struct timerData td(sem, ts); - hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&td, 1000*timeout); + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000*timeout); sem_wait(&sem); // Make sure timer thread is killed before sem goes diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index f53867a083..96a46beea5 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -421,10 +421,6 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) break; } -// FIXME - the calls to hrt_called and hrt_call_after seem not to work in the -// POSIX build -#ifndef __PX4_POSIX - /* * If the interval timer is still running, the topic should not * appear updated, even though at this point we know that it has. @@ -445,7 +441,6 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) sd->update_interval, &uORB::DeviceNode::update_deferred_trampoline, (void *)this); -#endif /* * Remember that we have told the subscriber that there is data. From 07efb655c4da5c0fcaa7b99a3524743f94169f7e Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 21:10:48 +0200 Subject: [PATCH 224/493] change start script to launch the px4flow driver in background. Fixes issue #2145 --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 536cfca91a..d9cdfa5dc0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -111,7 +111,7 @@ else fi # Check for flow sensor -if px4flow start +if px4flow start & then fi From d0b6c8f956b5bfb4176e618a7e4ff8271a289431 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 30 Jun 2015 15:20:04 -0700 Subject: [PATCH 225/493] GCC: Added fix for strict prototypes warning GCC requires a declaration of a static inline function prior to its definition when strict-prototypes warning is enabled. Signed-off-by: Mark Charlebois --- src/platforms/posix/include/hrt_work.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/posix/include/hrt_work.h b/src/platforms/posix/include/hrt_work.h index 39e53f95d2..4584baf258 100644 --- a/src/platforms/posix/include/hrt_work.h +++ b/src/platforms/posix/include/hrt_work.h @@ -46,12 +46,14 @@ void hrt_work_queue_init(void); int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); void hrt_work_cancel(struct work_s *work); +static inline void hrt_work_lock(void); static inline void hrt_work_lock() { //PX4_INFO("hrt_work_lock"); sem_wait(&_hrt_work_lock); } +static inline void hrt_work_unlock(void); static inline void hrt_work_unlock() { //PX4_INFO("hrt_work_unlock"); From c7e94baa5b2eea6d3312e07957bab186e27aeb00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jul 2015 12:56:22 +0200 Subject: [PATCH 226/493] Update SITL docs --- posix-configs/SITL/README.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/posix-configs/SITL/README.md b/posix-configs/SITL/README.md index 1bd38c0603..30cbd1aeba 100644 --- a/posix-configs/SITL/README.md +++ b/posix-configs/SITL/README.md @@ -18,6 +18,18 @@ Steps 1. Connect the RC Controller (PIXHAWK) to the PX4 machine using USB. Verify the `/dev/ttyACM0` device appears. Make sure that the persmissions of this device allow the PX4 app to open the device for read/write (`sudo chmod 777 /dev/ttyACM0`). +1. Run the quadrotor simulation: +``` +> make sitlrun +``` + +Detailed Background on System startup +--------------------------- + +NOTE: This is only necessary if you are not using the instructions above. + +1. Connect the RC Controller (PIXHAWK) to the PX4 machine using USB. Verify the `/dev/ttyACM0` device appears. Make sure that the persmissions of this device allow the PX4 app to open the device for read/write (`sudo chmod 777 /dev/ttyACM0`). + 1. Create the following diretories in "`./Firmware/Build/posix_sitl.build/`": ``` > cd ./Firmware/Build/posix_sitl.build From 1e46f441238c4cff30c6b5785a85ee9b818310b2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 19 Jun 2015 11:10:33 -0700 Subject: [PATCH 227/493] POSIX: ported systemcmds/tests Most of the systemcmds tests run in the posix build. The UART tests fail for me as I do not have a UART connected. Signed-off-by: Mark Charlebois --- makefiles/posix/config_posix_sitl.mk | 3 + src/modules/systemlib/module.mk | 1 - src/systemcmds/tests/module.mk | 12 ++- src/systemcmds/tests/test_adc.c | 19 ++--- src/systemcmds/tests/test_bson.c | 85 ++++++++++++--------- src/systemcmds/tests/test_conv.cpp | 6 +- src/systemcmds/tests/test_file.c | 20 ++--- src/systemcmds/tests/test_file2.c | 28 +++---- src/systemcmds/tests/test_float.c | 2 +- src/systemcmds/tests/test_gpio.c | 12 +-- src/systemcmds/tests/test_hott_telemetry.c | 51 ++++++++----- src/systemcmds/tests/test_hrt.c | 13 ++-- src/systemcmds/tests/test_int.c | 11 +-- src/systemcmds/tests/test_jig_voltages.c | 7 +- src/systemcmds/tests/test_led.c | 20 ++--- src/systemcmds/tests/test_mathlib.cpp | 39 +++++----- src/systemcmds/tests/test_mixer.cpp | 48 ++++++------ src/systemcmds/tests/test_mount.c | 36 ++++----- src/systemcmds/tests/test_param.c | 25 ++++-- src/systemcmds/tests/test_ppm_loopback.c | 1 - src/systemcmds/tests/test_rc.c | 15 ++-- src/systemcmds/tests/test_sensors.c | 28 +++---- src/systemcmds/tests/test_servo.c | 3 +- src/systemcmds/tests/test_sleep.c | 2 +- src/systemcmds/tests/test_time.c | 7 +- src/systemcmds/tests/test_uart_baudchange.c | 20 +++-- src/systemcmds/tests/test_uart_console.c | 1 - src/systemcmds/tests/test_uart_loopback.c | 2 +- src/systemcmds/tests/test_uart_send.c | 1 - src/systemcmds/tests/tests_main.c | 5 +- 30 files changed, 285 insertions(+), 238 deletions(-) diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index 806e59e7cb..b42581b96f 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -18,6 +18,9 @@ MODULES += modules/sensors # MODULES += systemcmds/param MODULES += systemcmds/mixer +#MODULES += systemcmds/esc_calib +MODULES += systemcmds/tests +#MODULES += systemcmds/reboot MODULES += systemcmds/topic_listener MODULES += systemcmds/ver diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index cc3d79c693..067cf7a91b 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -40,7 +40,6 @@ SRCS = \ param/param.c \ conversions.c \ cpuload.c \ - getopt_long.c \ pid/pid.c \ airspeed.c \ system_params.c \ diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 74114719fa..ff4d07f57b 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -18,7 +18,6 @@ SRCS = test_adc.c \ test_sensors.c \ test_servo.c \ test_sleep.c \ - test_time.c \ test_uart_baudchange.c \ test_uart_console.c \ test_uart_loopback.c \ @@ -35,5 +34,14 @@ SRCS = test_adc.c \ test_mount.c \ test_eigen.cpp -EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion -Wno-error=logical-op +ifeq ($(PX4_TARGET_OS), nuttx) +SRCS += test_time.c +endif + +EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal + +# Flag is only valid for GCC, not clang +ifneq ($(USE_GCC), 0) +EXTRACXXFLAGS += -Wno-double-promotion -Wno-error=logical-op +endif diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 7cb807d4c7..ef7f217fdf 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -37,7 +37,9 @@ */ #include -#include +#include +#include +#include #include @@ -46,22 +48,17 @@ #include #include #include -#include - -#include #include "tests.h" -#include #include -#include int test_adc(int argc, char *argv[]) { - int fd = open(ADC0_DEVICE_PATH, O_RDONLY); + int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - warnx("ERROR: can't open ADC device"); + PX4_ERR("ERROR: can't open ADC device"); return 1; } @@ -69,7 +66,7 @@ int test_adc(int argc, char *argv[]) /* make space for a maximum of twelve channels */ struct adc_msg_s data[12]; /* read all channels available */ - ssize_t count = read(fd, data, sizeof(data)); + ssize_t count = px4_read(fd, data, sizeof(data)); if (count < 0) { goto errout_with_dev; @@ -85,11 +82,11 @@ int test_adc(int argc, char *argv[]) usleep(150000); } - warnx("\t ADC test successful.\n"); + printf("\t ADC test successful.\n"); errout_with_dev: - if (fd != 0) { close(fd); } + if (fd != 0) { px4_close(fd); } return OK; } diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index 02384ebfeb..6309e23160 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -37,9 +37,14 @@ * Tests for the bson en/decoder */ +#define __STDC_FORMAT_MACROS +#include + +#include #include #include #include +#include #include #include @@ -59,27 +64,33 @@ static int encode(bson_encoder_t encoder) { if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0) { - warnx("FAIL: encoder: append bool failed"); + PX4_ERR("FAIL: encoder: append bool failed"); + return 1; } if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0) { - warnx("FAIL: encoder: append int failed"); + PX4_ERR("FAIL: encoder: append int failed"); + return 1; } if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0) { - warnx("FAIL: encoder: append int failed"); + PX4_ERR("FAIL: encoder: append int failed"); + return 1; } if (bson_encoder_append_double(encoder, "double1", sample_double) != 0) { - warnx("FAIL: encoder: append double failed"); + PX4_ERR("FAIL: encoder: append double failed"); + return 1; } if (bson_encoder_append_string(encoder, "string1", sample_string) != 0) { - warnx("FAIL: encoder: append string failed"); + PX4_ERR("FAIL: encoder: append string failed"); + return 1; } if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0) { - warnx("FAIL: encoder: append data failed"); + PX4_ERR("FAIL: encoder: append data failed"); + return 1; } bson_encoder_fini(encoder); @@ -94,29 +105,29 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "bool1")) { if (node->type != BSON_BOOL) { - warnx("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL); + PX4_ERR("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL); return 1; } if (node->b != sample_bool) { - warnx("FAIL: decoder: bool1 value %s, expected %s", + PX4_ERR("FAIL: decoder: bool1 value %s, expected %s", (node->b ? "true" : "false"), (sample_bool ? "true" : "false")); return 1; } - warnx("PASS: decoder: bool1"); + PX4_INFO("PASS: decoder: bool1"); return 1; } if (!strcmp(node->name, "int1")) { if (node->type != BSON_INT32) { - warnx("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32); + PX4_ERR("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32); return 1; } if (node->i != sample_small_int) { - warnx("FAIL: decoder: int1 value %lld, expected %d", node->i, sample_small_int); + PX4_ERR("FAIL: decoder: int1 value %" PRIu64 ", expected %d", node->i, sample_small_int); return 1; } @@ -126,12 +137,12 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "int2")) { if (node->type != BSON_INT64) { - warnx("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64); + PX4_ERR("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64); return 1; } if (node->i != sample_big_int) { - warnx("FAIL: decoder: int2 value %lld, expected %lld", node->i, sample_big_int); + PX4_ERR("FAIL: decoder: int2 value %" PRIu64 ", expected %" PRIu64, node->i, sample_big_int); return 1; } @@ -141,12 +152,12 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "double1")) { if (node->type != BSON_DOUBLE) { - warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); + PX4_ERR("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); return 1; } if (fabs(node->d - sample_double) > 1e-12) { - warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); + PX4_ERR("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); return 1; } @@ -156,36 +167,36 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "string1")) { if (node->type != BSON_STRING) { - warnx("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING); + PX4_ERR("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING); return 1; } len = bson_decoder_data_pending(decoder); if (len != strlen(sample_string) + 1) { - warnx("FAIL: decoder: string1 length %d wrong, expected %d", len, strlen(sample_string) + 1); + PX4_ERR("FAIL: decoder: string1 length %d wrong, expected %ld", len, strlen(sample_string) + 1); return 1; } char sbuf[len]; if (bson_decoder_copy_data(decoder, sbuf)) { - warnx("FAIL: decoder: string1 copy failed"); + PX4_ERR("FAIL: decoder: string1 copy failed"); return 1; } if (bson_decoder_data_pending(decoder) != 0) { - warnx("FAIL: decoder: string1 copy did not exhaust all data"); + PX4_ERR("FAIL: decoder: string1 copy did not exhaust all data"); return 1; } if (sbuf[len - 1] != '\0') { - warnx("FAIL: decoder: string1 not 0-terminated"); + PX4_ERR("FAIL: decoder: string1 not 0-terminated"); return 1; } if (strcmp(sbuf, sample_string)) { - warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string); + PX4_ERR("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string); return 1; } @@ -195,45 +206,45 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) if (!strcmp(node->name, "data1")) { if (node->type != BSON_BINDATA) { - warnx("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA); + PX4_ERR("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA); return 1; } len = bson_decoder_data_pending(decoder); if (len != sizeof(sample_data)) { - warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(sample_data)); + PX4_ERR("FAIL: decoder: data1 length %d, expected %lu", len, sizeof(sample_data)); return 1; } if (node->subtype != BSON_BIN_BINARY) { - warnx("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY); + PX4_ERR("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY); return 1; } uint8_t dbuf[len]; if (bson_decoder_copy_data(decoder, dbuf)) { - warnx("FAIL: decoder: data1 copy failed"); + PX4_ERR("FAIL: decoder: data1 copy failed"); return 1; } if (bson_decoder_data_pending(decoder) != 0) { - warnx("FAIL: decoder: data1 copy did not exhaust all data"); + PX4_ERR("FAIL: decoder: data1 copy did not exhaust all data"); return 1; } if (memcmp(sample_data, dbuf, len)) { - warnx("FAIL: decoder: data1 compare fail"); + PX4_ERR("FAIL: decoder: data1 compare fail"); return 1; } - warnx("PASS: decoder: data1"); + PX4_INFO("PASS: decoder: data1"); return 1; } if (node->type != BSON_EOO) { - warnx("FAIL: decoder: unexpected node name '%s'", node->name); + PX4_ERR("FAIL: decoder: unexpected node name '%s'", node->name); } return 1; @@ -259,29 +270,33 @@ test_bson(int argc, char *argv[]) /* encode data to a memory buffer */ if (bson_encoder_init_buf(&encoder, NULL, 0)) { - errx(1, "FAIL: bson_encoder_init_buf"); + PX4_ERR("FAIL: bson_encoder_init_buf"); + return 1; } encode(&encoder); len = bson_encoder_buf_size(&encoder); if (len <= 0) { - errx(1, "FAIL: bson_encoder_buf_len"); + PX4_ERR("FAIL: bson_encoder_buf_len"); + return 1; } buf = bson_encoder_buf_data(&encoder); if (buf == NULL) { - errx(1, "FAIL: bson_encoder_buf_data"); + PX4_ERR("FAIL: bson_encoder_buf_data"); + return 1; } /* now test-decode it */ if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL)) { - errx(1, "FAIL: bson_decoder_init_buf"); + PX4_ERR("FAIL: bson_decoder_init_buf"); + return 1; } decode(&decoder); free(buf); - return OK; -} \ No newline at end of file + return PX4_OK; +} diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 180c3f1032..8718342eb9 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -59,20 +59,20 @@ int test_conv(int argc, char *argv[]) { - warnx("Testing system conversions"); + PX4_INFO("Testing system conversions"); for (int i = -10000; i <= 10000; i += 1) { float f = i / 10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), + PX4_ERR("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); return 1; } } - warnx("All conversions clean"); + PX4_INFO("All conversions clean"); return 0; } diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index a43e01d6fb..bdb089e405 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -97,7 +97,7 @@ test_file(int argc, char *argv[]) /* check if microSD card is mounted */ struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { + if (stat(PX4_ROOTFSDIR "/fs/microsd/", &buffer)) { warnx("no microSD card mounted, aborting file test"); return 1; } @@ -125,7 +125,7 @@ test_file(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); hrt_abstime start, end; - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + int fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); warnx("testing unaligned writes - please wait.."); @@ -154,10 +154,10 @@ test_file(int argc, char *argv[]) end = hrt_absolute_time(); - warnx("write took %llu us", (end - start)); + warnx("write took %" PRIu64 " us", (end - start)); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY); /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { @@ -195,8 +195,8 @@ test_file(int argc, char *argv[]) */ close(fd); - int ret = unlink("/fs/microsd/testfile"); - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + int ret = unlink(PX4_ROOTFSDIR "/fs/microsd/testfile"); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); @@ -219,7 +219,7 @@ test_file(int argc, char *argv[]) warnx("reading data aligned.."); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY); bool align_read_ok = true; @@ -256,7 +256,7 @@ test_file(int argc, char *argv[]) warnx("reading data unaligned.."); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY); bool unalign_read_ok = true; int unalign_read_err_count = 0; @@ -297,7 +297,7 @@ test_file(int argc, char *argv[]) } - ret = unlink("/fs/microsd/testfile"); + ret = unlink(PX4_ROOTFSDIR "/fs/microsd/testfile"); close(fd); if (ret) { @@ -310,7 +310,7 @@ test_file(int argc, char *argv[]) /* list directory */ DIR *d; struct dirent *dir; - d = opendir("/fs/microsd"); + d = opendir(PX4_ROOTFSDIR "/fs/microsd"); if (d) { diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index 6adaa7709b..630b9adc48 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -37,17 +37,17 @@ * File write test. */ +#include #include #include #include #include #include #include -#include #include #include #include -#include +#include #include "tests.h" @@ -133,13 +133,13 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { printf("read failed at offset %u\n", ofs); - exit(1); + return; } for (uint16_t j = 0; j < write_chunk; j++) { if (buffer[j] != get_value(ofs)) { printf("corruption at ofs=%u got %u\n", ofs, buffer[j]); - exit(1); + return; } ofs++; @@ -170,11 +170,13 @@ int test_file2(int argc, char *argv[]) { int opt; uint16_t flags = 0; - const char *filename = "/fs/microsd/testfile2.dat"; + const char *filename = PX4_ROOTFSDIR "/fs/microsd/testfile2.dat"; uint32_t write_chunk = 64; uint32_t write_size = 5 * 1024; - while ((opt = getopt(argc, argv, "c:s:FLh")) != EOF) { + int myoptind = 1; + const char *myoptarg = NULL; + while ((opt = px4_getopt(argc, argv, "c:s:FLh", &myoptind, &myoptarg)) != EOF) { switch (opt) { case 'F': flags |= FLAG_FSYNC; @@ -185,22 +187,22 @@ int test_file2(int argc, char *argv[]) break; case 's': - write_size = strtoul(optarg, NULL, 0); + write_size = strtoul(myoptarg, NULL, 0); break; case 'c': - write_chunk = strtoul(optarg, NULL, 0); + write_chunk = strtoul(myoptarg, NULL, 0); break; case 'h': default: usage(); - exit(1); + return 1; } } - argc -= optind; - argv += optind; + argc -= myoptind; + argv += myoptind; if (argc > 0) { filename = argv[0]; @@ -209,8 +211,8 @@ int test_file2(int argc, char *argv[]) /* check if microSD card is mounted */ struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); + if (stat(PX4_ROOTFSDIR "/fs/microsd/", &buffer)) { + fprintf(stderr, "no microSD card mounted, aborting file test"); return 1; } diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index 55f466e734..83b102b41e 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -39,12 +39,12 @@ #include #include +#include #include #include #include #include #include -#include #include "tests.h" #include #include diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c index e14b4b4e6c..3d27d903f0 100644 --- a/src/systemcmds/tests/test_gpio.c +++ b/src/systemcmds/tests/test_gpio.c @@ -37,6 +37,7 @@ ****************************************************************************/ #include +#include #include @@ -45,9 +46,8 @@ #include #include #include -#include -#include +#include #include "tests.h" @@ -93,7 +93,7 @@ int test_gpio(int argc, char *argv[]) #ifdef PX4IO_DEVICE_PATH - int fd = open(PX4IO_DEVICE_PATH, 0); + int fd = px4_open(PX4IO_DEVICE_PATH, 0); if (fd < 0) { printf("GPIO: open fail\n"); @@ -101,16 +101,16 @@ int test_gpio(int argc, char *argv[]) } /* set all GPIOs to default state */ - ioctl(fd, GPIO_RESET, ~0); + px4_ioctl(fd, GPIO_RESET, ~0); /* XXX need to add some GPIO waving stuff here */ /* Go back to default */ - ioctl(fd, GPIO_RESET, ~0); + px4_ioctl(fd, GPIO_RESET, ~0); - close(fd); + px4_close(fd); printf("\t GPIO test successful.\n"); #endif diff --git a/src/systemcmds/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c index 281e7e5029..430d0445d8 100644 --- a/src/systemcmds/tests/test_hott_telemetry.c +++ b/src/systemcmds/tests/test_hott_telemetry.c @@ -45,10 +45,11 @@ #include #include +#include +#include #include #include -#include #include #include #include @@ -92,7 +93,7 @@ static int open_uart(const char *device) int uart = open(device, O_RDWR | O_NOCTTY); if (uart < 0) { - errx(1, "FAIL: Error opening port"); + PX4_ERR("FAIL: Error opening port"); return ERROR; } @@ -107,12 +108,12 @@ static int open_uart(const char *device) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - errx(1, "FAIL: Error setting baudrate / termios config for cfsetispeed, cfsetospeed"); + PX4_ERR("FAIL: Error setting baudrate / termios config for cfsetispeed, cfsetospeed"); return ERROR; } if (tcsetattr(uart, TCSANOW, &uart_config) < 0) { - errx(1, "FAIL: Error setting baudrate / termios config for tcsetattr"); + PX4_ERR("FAIL: Error setting baudrate / termios config for tcsetattr"); return ERROR; } @@ -129,11 +130,11 @@ static int open_uart(const char *device) int test_hott_telemetry(int argc, char *argv[]) { - warnx("HoTT Telemetry Test Requirements:"); - warnx("- Radio on and Electric Air. Mod on (telemetry -> sensor select)."); - warnx("- Receiver telemetry port must be in telemetry mode."); - warnx("- Connect telemetry wire to /dev/ttyS1 (USART2)."); - warnx("Testing..."); + PX4_INFO("HoTT Telemetry Test Requirements:"); + PX4_INFO("- Radio on and Electric Air. Mod on (telemetry -> sensor select)."); + PX4_INFO("- Receiver telemetry port must be in telemetry mode."); + PX4_INFO("- Connect telemetry wire to /dev/ttyS1 (USART2)."); + PX4_INFO("Testing..."); const char device[] = "/dev/ttyS1"; int fd = open_uart(device); @@ -143,8 +144,10 @@ int test_hott_telemetry(int argc, char *argv[]) return ERROR; } +#ifdef TIOCSSINGLEWIRE /* Activate single wire mode */ ioctl(fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +#endif char send = 'a'; write(fd, &send, 1); @@ -154,12 +157,13 @@ int test_hott_telemetry(int argc, char *argv[]) struct pollfd fds[] = { { .fd = fd, .events = POLLIN } }; if (poll(fds, 1, timeout) == 0) { - errx(1, "FAIL: Could not read sent data."); + PX4_ERR("FAIL: Could not read sent data."); + return 1; } char receive; read(fd, &receive, 1); - warnx("PASS: Single wire enabled. Sent %x and received %x", send, receive); + PX4_INFO("PASS: Single wire enabled. Sent %x and received %x", send, receive); /* Attempt to read HoTT poll messages from the HoTT receiver */ @@ -170,8 +174,8 @@ int test_hott_telemetry(int argc, char *argv[]) for (; received_count < 5; received_count++) { if (poll(fds, 1, timeout) == 0) { - errx(1, "FAIL: Could not read sent data. Is your HoTT receiver plugged in on %s?", device); - break; + PX4_ERR("FAIL: Could not read sent data. Is your HoTT receiver plugged in on %s?", device); + return 1; } else { read(fd, &byte, 1); @@ -187,21 +191,23 @@ int test_hott_telemetry(int argc, char *argv[]) if (received_count > 0 && valid_count > 0) { if (received_count == max_polls && valid_count == max_polls) { - warnx("PASS: Received %d out of %d valid byte pairs from the HoTT receiver device.", received_count, max_polls); + PX4_INFO("PASS: Received %d out of %d valid byte pairs from the HoTT receiver device.", received_count, max_polls); } else { - warnx("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, + PX4_WARN("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, max_polls, valid_count); } } else { /* Let's work out what went wrong */ if (received_count == 0) { - errx(1, "FAIL: Could not read any polls from HoTT receiver device."); + PX4_ERR("FAIL: Could not read any polls from HoTT receiver device."); + return 1; } if (valid_count == 0) { - errx(1, "FAIL: Received unexpected values from the HoTT receiver device."); + PX4_ERR("FAIL: Received unexpected values from the HoTT receiver device."); + return 1; } } @@ -221,21 +227,24 @@ int test_hott_telemetry(int argc, char *argv[]) usleep(1000); } - warnx("PASS: Response sent to the HoTT receiver device. Voltage should now show 2.5V."); + PX4_INFO("PASS: Response sent to the HoTT receiver device. Voltage should now show 2.5V."); +#ifdef TIOCSSINGLEWIRE /* Disable single wire */ ioctl(fd, TIOCSSINGLEWIRE, ~SER_SINGLEWIRE_ENABLED); +#endif write(fd, &send, 1); /* We should timeout as there will be nothing to read (TX and RX no longer connected) */ if (poll(fds, 1, timeout) == 0) { - errx(1, "FAIL: timeout expected."); + PX4_ERR("FAIL: timeout expected."); + return 1; } - warnx("PASS: Single wire disabled."); + PX4_INFO("PASS: Single wire disabled."); close(fd); - exit(0); + return 0; } diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index 40043c69be..8bc0888122 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -46,7 +46,6 @@ #include #include #include -#include #include #include @@ -54,7 +53,7 @@ #include #include -#include +//#include #include "tests.h" @@ -127,7 +126,7 @@ int test_tone(int argc, char *argv[]) int fd, result; unsigned long tone; - fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); + fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); if (fd < 0) { printf("failed opening " TONEALARM0_DEVICE_PATH "\n"); @@ -141,7 +140,7 @@ int test_tone(int argc, char *argv[]) } if (tone == 0) { - result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); + result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); @@ -152,14 +151,14 @@ int test_tone(int argc, char *argv[]) } } else { - result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); + result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); goto out; } - result = ioctl(fd, TONE_SET_ALARM, tone); + result = px4_ioctl(fd, TONE_SET_ALARM, tone); if (result < 0) { printf("failed setting alarm %lu\n", tone); @@ -172,7 +171,7 @@ int test_tone(int argc, char *argv[]) out: if (fd >= 0) { - close(fd); + px4_close(fd); } return 0; diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c index 95ee7652dd..f9a24d6843 100644 --- a/src/systemcmds/tests/test_int.c +++ b/src/systemcmds/tests/test_int.c @@ -42,10 +42,11 @@ #include #include +#include +#include #include #include #include -#include #include @@ -105,10 +106,10 @@ int test_int(int argc, char *argv[]) int64_t calc = large * 5; if (calc == 1770781647990) { - printf("\t success: 354156329598 * 5 == %lld\n", calc); + printf("\t success: 354156329598 * 5 == %" PRId64 "\n", calc); } else { - printf("\t FAIL: 354156329598 * 5 != %lld\n", calc); + printf("\t FAIL: 354156329598 * 5 != %" PRId64 "\n", calc); ret = -1; } @@ -127,10 +128,10 @@ int test_int(int argc, char *argv[]) uint64_t small_times_large = large_int * (uint64_t)small; if (small_times_large == 107374182350) { - printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %lld\n", small_times_large); + printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %" PRId64 "\n", small_times_large); } else { - printf("\t FAIL: 50 * 2147483647 != %lld, 64bit cast might fail\n", small_times_large); + printf("\t FAIL: 50 * 2147483647 != %" PRId64 ", 64bit cast might fail\n", small_times_large); ret = -1; } diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c index a04aacc3a7..f94caa87b3 100644 --- a/src/systemcmds/tests/test_jig_voltages.c +++ b/src/systemcmds/tests/test_jig_voltages.c @@ -36,7 +36,7 @@ ****************************************************************************/ #include -#include +#include #include @@ -45,13 +45,12 @@ #include #include #include -#include -#include +//#include #include "tests.h" -#include +#include #include #include diff --git a/src/systemcmds/tests/test_led.c b/src/systemcmds/tests/test_led.c index f56660b74a..f045640453 100644 --- a/src/systemcmds/tests/test_led.c +++ b/src/systemcmds/tests/test_led.c @@ -37,6 +37,7 @@ ****************************************************************************/ #include +#include #include @@ -45,7 +46,6 @@ #include #include #include -#include #include @@ -91,15 +91,15 @@ int test_led(int argc, char *argv[]) int fd; int ret = 0; - fd = open(LED0_DEVICE_PATH, 0); + fd = px4_open(LED0_DEVICE_PATH, 0); if (fd < 0) { printf("\tLED: open fail\n"); return ERROR; } - if (ioctl(fd, LED_ON, LED_BLUE) || - ioctl(fd, LED_ON, LED_AMBER)) { + if (px4_ioctl(fd, LED_ON, LED_BLUE) || + px4_ioctl(fd, LED_ON, LED_AMBER)) { printf("\tLED: ioctl fail\n"); return ERROR; @@ -112,12 +112,12 @@ int test_led(int argc, char *argv[]) for (i = 0; i < 10; i++) { if (ledon) { - ioctl(fd, LED_ON, LED_BLUE); - ioctl(fd, LED_OFF, LED_AMBER); + px4_ioctl(fd, LED_ON, LED_BLUE); + px4_ioctl(fd, LED_OFF, LED_AMBER); } else { - ioctl(fd, LED_OFF, LED_BLUE); - ioctl(fd, LED_ON, LED_AMBER); + px4_ioctl(fd, LED_OFF, LED_BLUE); + px4_ioctl(fd, LED_ON, LED_AMBER); } ledon = !ledon; @@ -125,8 +125,8 @@ int test_led(int argc, char *argv[]) } /* Go back to default */ - ioctl(fd, LED_ON, LED_BLUE); - ioctl(fd, LED_OFF, LED_AMBER); + px4_ioctl(fd, LED_ON, LED_BLUE); + px4_ioctl(fd, LED_OFF, LED_AMBER); printf("\t LED test completed, no errors.\n"); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 7460f6f559..71c2ae5b93 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -37,6 +37,7 @@ * Mathlib test */ +#include #include #include #include @@ -48,14 +49,14 @@ #include "tests.h" -#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } +#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); } using namespace math; int test_mathlib(int argc, char *argv[]) { int rc = 0; - warnx("testing mathlib"); + PX4_INFO("testing mathlib"); { Vector<2> v; @@ -158,7 +159,7 @@ int test_mathlib(int argc, char *argv[]) } { - warnx("Nonsymmetric matrix operations test"); + PX4_INFO("Nonsymmetric matrix operations test"); // test nonsymmetric +, -, +=, -= float data1[2][3] = {{1, 2, 3}, {4, 5, 6}}; @@ -170,7 +171,7 @@ int test_mathlib(int argc, char *argv[]) Matrix<2, 3> m3(data3); if (m1 + m2 != m3) { - warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + PX4_ERR("Matrix<2, 3> + Matrix<2, 3> failed!"); (m1 + m2).print(); printf("!=\n"); m3.print(); @@ -178,7 +179,7 @@ int test_mathlib(int argc, char *argv[]) } if (m3 - m2 != m1) { - warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!"); (m3 - m2).print(); printf("!=\n"); m1.print(); @@ -188,7 +189,7 @@ int test_mathlib(int argc, char *argv[]) m1 += m2; if (m1 != m3) { - warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + PX4_ERR("Matrix<2, 3> += Matrix<2, 3> failed!"); m1.print(); printf("!=\n"); m3.print(); @@ -199,7 +200,7 @@ int test_mathlib(int argc, char *argv[]) Matrix<2, 3> m1_orig(data1); if (m1 != m1_orig) { - warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + PX4_ERR("Matrix<2, 3> -= Matrix<2, 3> failed!"); m1.print(); printf("!=\n"); m1_orig.print(); @@ -216,7 +217,7 @@ int test_mathlib(int argc, char *argv[]) float diff = 0.1f; float tol = 0.00001f; - warnx("Quaternion transformation methods test."); + PX4_INFO("Quaternion transformation methods test."); for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { @@ -228,7 +229,7 @@ int test_mathlib(int argc, char *argv[]) for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) { - warnx("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!"); + PX4_WARN("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!"); rc = 1; } } @@ -245,7 +246,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 4; i++) { if (fabsf(q.data[i] - q_true.data[i]) > tol) { - warnx("Quaternion method 'from_dcm()' outside tolerance!"); + PX4_WARN("Quaternion method 'from_dcm()' outside tolerance!"); rc = 1; } } @@ -255,7 +256,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 4; i++) { if (fabsf(q.data[i] - q_true.data[i]) > tol) { - warnx("Quaternion method 'from_euler()' outside tolerance!"); + PX4_WARN("Quaternion method 'from_euler()' outside tolerance!"); rc = 1; } } @@ -265,7 +266,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 4; i++) { if (fabsf(q.data[i] - q_true.data[i]) > tol) { - warnx("Quaternion method 'from_euler()' outside tolerance!"); + PX4_WARN("Quaternion method 'from_euler()' outside tolerance!"); rc = 1; } } @@ -275,7 +276,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 4; i++) { if (fabsf(q.data[i] - q_true.data[i]) > tol) { - warnx("Quaternion method 'from_euler()' outside tolerance!"); + PX4_WARN("Quaternion method 'from_euler()' outside tolerance!"); rc = 1; } } @@ -292,7 +293,7 @@ int test_mathlib(int argc, char *argv[]) float diff = 0.1f; float tol = 0.00001f; - warnx("Quaternion vector rotation method test."); + PX4_INFO("Quaternion vector rotation method test."); for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { @@ -304,7 +305,7 @@ int test_mathlib(int argc, char *argv[]) for (int i = 0; i < 3; i++) { if (fabsf(vector_r(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } @@ -320,7 +321,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } @@ -331,7 +332,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } @@ -342,7 +343,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } @@ -353,7 +354,7 @@ int test_mathlib(int argc, char *argv[]) for (unsigned i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); + PX4_WARN("Quaternion method 'rotate' outside tolerance"); rc = 1; } } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index d0f76d3b69..3f8e924db3 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -80,7 +80,7 @@ int test_mixer(int argc, char *argv[]) uint16_t servo_predicted[output_max]; int16_t reverse_pwm_mask = 0; - warnx("testing mixer"); + PX4_INFO("testing mixer"); const char *filename = "/etc/mixers/IO_pass.mix"; @@ -88,14 +88,14 @@ int test_mixer(int argc, char *argv[]) filename = argv[1]; } - warnx("loading: %s", filename); + PX4_INFO("loading: %s", filename); char buf[2048]; load_mixer_file(filename, &buf[0], sizeof(buf)); unsigned loaded = strlen(buf); - warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + PX4_INFO("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); /* load the mixer in chunks, like * in the case of a remote load, @@ -109,7 +109,7 @@ int test_mixer(int argc, char *argv[]) /* load at once test */ unsigned xx = loaded; mixer_group.load_from_buf(&buf[0], xx); - warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count()); if (mixer_group.count() != 8) { return 1; @@ -121,7 +121,7 @@ int test_mixer(int argc, char *argv[]) empty_buf[1] = '\0'; mixer_group.reset(); mixer_group.load_from_buf(&empty_buf[0], empty_load); - warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); + PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); if (empty_load != 0) { return 1; @@ -135,7 +135,7 @@ int test_mixer(int argc, char *argv[]) unsigned transmitted = 0; - warnx("transmitted: %d, loaded: %d", transmitted, loaded); + PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded); while (transmitted < loaded) { @@ -150,7 +150,7 @@ int test_mixer(int argc, char *argv[]) memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; - warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); + PX4_INFO("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); /* process the text buffer, adding new mixers as their descriptions can be parsed */ unsigned resid = mixer_text_length; @@ -158,7 +158,7 @@ int test_mixer(int argc, char *argv[]) /* if anything was parsed */ if (resid != mixer_text_length) { - warnx("used %u", mixer_text_length - resid); + PX4_INFO("used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ if (resid > 0) { @@ -171,7 +171,7 @@ int test_mixer(int argc, char *argv[]) transmitted += text_length; } - warnx("chunked load: loaded %u mixers", mixer_group.count()); + PX4_INFO("chunked load: loaded %u mixers", mixer_group.count()); if (mixer_group.count() != 8) { return 1; @@ -194,7 +194,7 @@ int test_mixer(int argc, char *argv[]) r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } - warnx("ARMING TEST: STARTING RAMP"); + PX4_INFO("ARMING TEST: STARTING RAMP"); unsigned sleep_quantum_us = 10000; hrt_abstime starttime = hrt_absolute_time(); @@ -213,13 +213,13 @@ int test_mixer(int argc, char *argv[]) /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && r_page_servos[i] != r_page_servo_disarmed[i]) { - warnx("disarmed servo value mismatch"); + PX4_ERR("disarmed servo value mismatch"); return 1; } if (hrt_elapsed_time(&starttime) >= INIT_TIME_US && r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { - warnx("ramp servo value mismatch"); + PX4_ERR("ramp servo value mismatch"); return 1; } @@ -237,7 +237,7 @@ int test_mixer(int argc, char *argv[]) printf("\n"); - warnx("ARMING TEST: NORMAL OPERATION"); + PX4_INFO("ARMING TEST: NORMAL OPERATION"); for (int j = -jmax; j <= jmax; j++) { @@ -254,20 +254,20 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - warnx("mixed %d outputs (max %d)", mixed, output_max); + PX4_INFO("mixed %d outputs (max %d)", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); - warnx("mixer violated predicted value"); + PX4_ERR("mixer violated predicted value"); return 1; } } } - warnx("ARMING TEST: DISARMING"); + PX4_INFO("ARMING TEST: DISARMING"); starttime = hrt_absolute_time(); sleepcount = 0; @@ -285,7 +285,7 @@ int test_mixer(int argc, char *argv[]) for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { - warnx("disarmed servo value mismatch"); + PX4_ERR("disarmed servo value mismatch"); return 1; } @@ -303,7 +303,7 @@ int test_mixer(int argc, char *argv[]) printf("\n"); - warnx("ARMING TEST: REARMING: STARTING RAMP"); + PX4_INFO("ARMING TEST: REARMING: STARTING RAMP"); starttime = hrt_absolute_time(); sleepcount = 0; @@ -327,7 +327,7 @@ int test_mixer(int argc, char *argv[]) if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || r_page_servos[i] > servo_predicted[i])) { - warnx("ramp servo value mismatch"); + PX4_ERR("ramp servo value mismatch"); return 1; } @@ -335,7 +335,7 @@ int test_mixer(int argc, char *argv[]) if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); - warnx("mixer violated predicted value"); + PX4_ERR("mixer violated predicted value"); return 1; } @@ -366,18 +366,18 @@ int test_mixer(int argc, char *argv[]) load_mixer_file(filename, &buf[0], sizeof(buf)); loaded = strlen(buf); - warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + PX4_INFO("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); unsigned mc_loaded = loaded; mixer_group.load_from_buf(&buf[0], mc_loaded); - warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count()); if (mixer_group.count() != 5) { - warnx("FAIL: Quad W mixer load failed"); + PX4_ERR("FAIL: Quad W mixer load failed"); return 1; } - warnx("SUCCESS: No errors in mixer test"); + PX4_INFO("SUCCESS: No errors in mixer test"); return 0; } diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 2634965d28..64eefa0b47 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -69,15 +69,15 @@ test_mount(int argc, char *argv[]) /* check if microSD card is mounted */ struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); + if (stat(PX4_ROOTFSDIR "/fs/microsd/", &buffer)) { + PX4_ERR("no microSD card mounted, aborting file test"); return 1; } /* list directory */ DIR *d; struct dirent *dir; - d = opendir("/fs/microsd"); + d = opendir(PX4_ROOTFSDIR "/fs/microsd"); if (d) { @@ -87,11 +87,11 @@ test_mount(int argc, char *argv[]) closedir(d); - warnx("directory listing ok (FS mounted and readable)"); + PX4_INFO("directory listing ok (FS mounted and readable)"); } else { /* failed opening dir */ - warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); + PX4_ERR("FAILED LISTING MICROSD ROOT DIRECTORY"); if (stat(cmd_filename, &buffer) == OK) { (void)unlink(cmd_filename); @@ -131,7 +131,7 @@ test_mount(int argc, char *argv[]) it_left_abort = abort_tries; } - warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, + PX4_INFO("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, fsync_tries, abort_tries, buf); int it_left_fsync_prev = it_left_fsync; @@ -147,7 +147,7 @@ test_mount(int argc, char *argv[]) /* announce mode switch */ if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) { - warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); + PX4_INFO("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); fsync(fileno(stdout)); fsync(fileno(stderr)); usleep(20000); @@ -165,7 +165,7 @@ test_mount(int argc, char *argv[]) /* this must be the first iteration, do something */ cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT, PX4_O_MODE_666); - warnx("First iteration of file test\n"); + PX4_INFO("First iteration of file test\n"); } char buf[64]; @@ -200,17 +200,17 @@ test_mount(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + int fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf + a, chunk_sizes[c]); if (wret != (int)chunk_sizes[c]) { - warn("WRITE ERROR!"); + PX4_ERR("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) { - warnx("memory is unaligned, align shift: %d", a); + PX4_ERR("memory is unaligned, align shift: %d", a); } return 1; @@ -237,14 +237,14 @@ test_mount(int argc, char *argv[]) usleep(200000); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY); /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != (int)chunk_sizes[c]) { - warnx("READ ERROR!"); + PX4_ERR("READ ERROR!"); return 1; } @@ -253,24 +253,24 @@ test_mount(int argc, char *argv[]) for (unsigned j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j + a]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + PX4_WARN("COMPARISON ERROR: byte %d, align shift: %d", j, a); compare_ok = false; break; } } if (!compare_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + PX4_ERR("ABORTING FURTHER COMPARISON DUE TO ERROR"); return 1; } } - int ret = unlink("/fs/microsd/testfile"); + int ret = unlink(PX4_ROOTFSDIR "/fs/microsd/testfile"); close(fd); if (ret) { - warnx("UNLINKING FILE FAILED"); + PX4_ERR("UNLINKING FILE FAILED"); return 1; } @@ -284,7 +284,7 @@ test_mount(int argc, char *argv[]) /* we always reboot for the next test if we get here */ - warnx("Iteration done, rebooting.."); + PX4_INFO("Iteration done, rebooting.."); fsync(fileno(stdout)); fsync(fileno(stderr)); usleep(50000); diff --git a/src/systemcmds/tests/test_param.c b/src/systemcmds/tests/test_param.c index ab41841905..09845ee25c 100644 --- a/src/systemcmds/tests/test_param.c +++ b/src/systemcmds/tests/test_param.c @@ -37,6 +37,7 @@ * Tests related to the parameter system. */ +#include #include #include "systemlib/err.h" #include "systemlib/param/param.h" @@ -54,41 +55,49 @@ test_param(int argc, char *argv[]) p = param_find("test"); if (p == PARAM_INVALID) { - errx(1, "test parameter not found"); + warnx("test parameter not found"); + return 1; } if (param_reset(p) != OK) { - errx(1, "failed param reset"); + warnx("failed param reset"); + return 1; } param_type_t t = param_type(p); if (t != PARAM_TYPE_INT32) { - errx(1, "test parameter type mismatch (got %u)", (unsigned)t); + warnx("test parameter type mismatch (got %u)", (unsigned)t); + return 1; } int32_t val; if (param_get(p, &val) != OK) { - errx(1, "failed to read test parameter"); + warnx("failed to read test parameter"); + return 1; } if (val != PARAM_MAGIC1) { - errx(1, "parameter value mismatch"); + warnx("parameter value mismatch"); + return 1; } val = PARAM_MAGIC2; if (param_set(p, &val) != OK) { - errx(1, "failed to write test parameter"); + warnx("failed to write test parameter"); + return 1; } if (param_get(p, &val) != OK) { - errx(1, "failed to re-read test parameter"); + warnx("failed to re-read test parameter"); + return 1; } if ((uint32_t)val != PARAM_MAGIC2) { - errx(1, "parameter value mismatch after write"); + warnx("parameter value mismatch after write"); + return 1; } warnx("parameter test PASS"); diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index 7f1323f2b9..6f4ef9bbc8 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -46,7 +46,6 @@ #include #include #include -#include #include #include diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index fed11c8cb1..b4f68c35fb 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -47,7 +47,6 @@ #include #include #include -#include #include #include @@ -76,8 +75,8 @@ int test_rc(int argc, char *argv[]) bool rc_updated; orb_check(_rc_sub, &rc_updated); - warnx("Reading PPM values - press any key to abort"); - warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes."); + PX4_INFO("Reading PPM values - press any key to abort"); + PX4_INFO("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes."); if (rc_updated) { @@ -108,7 +107,7 @@ int test_rc(int argc, char *argv[]) /* go and check values */ for (unsigned i = 0; i < rc_input.channel_count; i++) { if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) { - warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]); + PX4_ERR("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]); (void)close(_rc_sub); return ERROR; } @@ -117,13 +116,13 @@ int test_rc(int argc, char *argv[]) } if (rc_last.channel_count != rc_input.channel_count) { - warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count); + PX4_ERR("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count); (void)close(_rc_sub); return ERROR; } if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) { - warnx("TIMEOUT, less than 10 Hz updates"); + PX4_ERR("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; } @@ -137,11 +136,11 @@ int test_rc(int argc, char *argv[]) } } else { - warnx("failed reading RC input data"); + PX4_ERR("failed reading RC input data"); return ERROR; } - warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!"); + PX4_INFO("PPM CONTINUITY TEST PASSED SUCCESSFULLY!"); return 0; } diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 8d27247f79..d663a7c3f0 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -39,6 +39,7 @@ */ #include +#include #include @@ -47,13 +48,12 @@ #include #include #include -#include #include #include #include -#include +//#include #include "tests.h" @@ -95,7 +95,7 @@ accel(int argc, char *argv[], const char *path) struct accel_report buf; int ret; - fd = open(path, O_RDONLY); + fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tACCEL: open fail, run or or first.\n"); @@ -106,7 +106,7 @@ accel(int argc, char *argv[], const char *path) usleep(100000); /* read data - expect samples */ - ret = read(fd, &buf, sizeof(buf)); + ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tACCEL: read1 fail (%d)\n", ret); @@ -130,7 +130,7 @@ accel(int argc, char *argv[], const char *path) /* Let user know everything is ok */ printf("\tOK: ACCEL passed all tests successfully\n"); - close(fd); + px4_close(fd); return OK; } @@ -145,7 +145,7 @@ gyro(int argc, char *argv[], const char *path) struct gyro_report buf; int ret; - fd = open(path, O_RDONLY); + fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tGYRO: open fail, run or first.\n"); @@ -156,7 +156,7 @@ gyro(int argc, char *argv[], const char *path) usleep(5000); /* read data - expect samples */ - ret = read(fd, &buf, sizeof(buf)); + ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tGYRO: read fail (%d)\n", ret); @@ -175,7 +175,7 @@ gyro(int argc, char *argv[], const char *path) /* Let user know everything is ok */ printf("\tOK: GYRO passed all tests successfully\n"); - close(fd); + px4_close(fd); return OK; } @@ -190,7 +190,7 @@ mag(int argc, char *argv[], const char *path) struct mag_report buf; int ret; - fd = open(path, O_RDONLY); + fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tMAG: open fail, run or first.\n"); @@ -201,7 +201,7 @@ mag(int argc, char *argv[], const char *path) usleep(5000); /* read data - expect samples */ - ret = read(fd, &buf, sizeof(buf)); + ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tMAG: read fail (%d)\n", ret); @@ -220,7 +220,7 @@ mag(int argc, char *argv[], const char *path) /* Let user know everything is ok */ printf("\tOK: MAG passed all tests successfully\n"); - close(fd); + px4_close(fd); return OK; } @@ -235,7 +235,7 @@ baro(int argc, char *argv[], const char *path) struct baro_report buf; int ret; - fd = open(path, O_RDONLY); + fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tBARO: open fail, run or first.\n"); @@ -246,7 +246,7 @@ baro(int argc, char *argv[], const char *path) usleep(5000); /* read data - expect samples */ - ret = read(fd, &buf, sizeof(buf)); + ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tBARO: read fail (%d)\n", ret); @@ -259,7 +259,7 @@ baro(int argc, char *argv[], const char *path) /* Let user know everything is ok */ printf("\tOK: BARO passed all tests successfully\n"); - close(fd); + px4_close(fd); return OK; } diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index 7dea9ac549..a862d41d1a 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -46,14 +46,13 @@ #include #include #include -#include #include #include #include #include -#include +//#include #include "tests.h" diff --git a/src/systemcmds/tests/test_sleep.c b/src/systemcmds/tests/test_sleep.c index e21763f43f..09f2bd2dca 100644 --- a/src/systemcmds/tests/test_sleep.c +++ b/src/systemcmds/tests/test_sleep.c @@ -37,6 +37,7 @@ ****************************************************************************/ #include +#include #include @@ -45,7 +46,6 @@ #include #include #include -#include #include diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c index 48e084326b..d89f90c683 100644 --- a/src/systemcmds/tests/test_time.c +++ b/src/systemcmds/tests/test_time.c @@ -45,7 +45,6 @@ #include #include #include -#include #include @@ -104,7 +103,7 @@ cycletime(void) ****************************************************************************/ /**************************************************************************** - * Name: test_led + * Name: test_time ****************************************************************************/ int test_time(int argc, char *argv[]) @@ -153,11 +152,11 @@ int test_time(int argc, char *argv[]) } if (deltadelta > 1000) { - fprintf(stderr, "h %llu c %llu d %lld\n", h, c, delta - lowdelta); + fprintf(stderr, "h %" PRIu64 " c %" PRIu64 " d %" PRId64 "\n", h, c, delta - lowdelta); } } - printf("Maximum jitter %lldus\n", maxdelta); + printf("Maximum jitter %" PRId64 "us\n", maxdelta); return 0; } diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c index 5ee2b6c5b5..8c9a5ff35f 100644 --- a/src/systemcmds/tests/test_uart_baudchange.c +++ b/src/systemcmds/tests/test_uart_baudchange.c @@ -38,6 +38,7 @@ ****************************************************************************/ #include +#include #include @@ -46,7 +47,6 @@ #include #include #include -#include #include #include @@ -109,12 +109,15 @@ int test_uart_baudchange(int argc, char *argv[]) int termios_state = 0; + int ret; + #define UART_BAUDRATE_RUNTIME_CONF #ifdef UART_BAUDRATE_RUNTIME_CONF if ((termios_state = tcgetattr(uart2, &uart2_config)) < 0) { printf("ERROR getting termios config for UART2: %d\n", termios_state); - exit(termios_state); + ret = termios_state; + goto cleanup; } memcpy(&uart2_config_original, &uart2_config, sizeof(struct termios)); @@ -122,18 +125,21 @@ int test_uart_baudchange(int argc, char *argv[]) /* Set baud rate */ if (cfsetispeed(&uart2_config, B9600) < 0 || cfsetospeed(&uart2_config, B9600) < 0) { printf("ERROR setting termios config for UART2: %d\n", termios_state); - exit(ERROR); + ret = ERROR; + goto cleanup; } if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config)) < 0) { printf("ERROR setting termios config for UART2\n"); - exit(termios_state); + ret = termios_state; + goto cleanup; } /* Set back to original settings */ if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config_original)) < 0) { printf("ERROR setting termios config for UART2\n"); - exit(termios_state); + ret = termios_state; + goto cleanup; } #endif @@ -156,4 +162,8 @@ int test_uart_baudchange(int argc, char *argv[]) printf("uart2_nwrite %d\n", uart2_nwrite); return OK; +cleanup: + close(uart2); + return ret; + } diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c index e49fc1f94c..ad1b2776e1 100644 --- a/src/systemcmds/tests/test_uart_console.c +++ b/src/systemcmds/tests/test_uart_console.c @@ -46,7 +46,6 @@ #include #include #include -#include #include diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index a929a6f94d..a45254bf93 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -43,10 +43,10 @@ #include #include +#include #include #include #include -#include #include diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c index ce63071ac2..bc4a0bddad 100644 --- a/src/systemcmds/tests/test_uart_send.c +++ b/src/systemcmds/tests/test_uart_send.c @@ -46,7 +46,6 @@ #include #include #include -#include #include diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 546f3ceb57..e16e0adb56 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -48,11 +48,10 @@ #include #include #include -#include #include -#include +//#include #include @@ -97,7 +96,9 @@ const struct { {"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST}, {"tone", test_tone, 0}, {"sleep", test_sleep, OPT_NOJIGTEST}, +#ifdef __PX4_NUTTX {"time", test_time, OPT_NOJIGTEST}, +#endif {"perf", test_perf, OPT_NOJIGTEST}, {"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST}, {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST}, From 63f7995b4158f5aaf158b7690508b6def98e9ffb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 19 Jun 2015 11:28:47 -0700 Subject: [PATCH 228/493] NuttX: fixes for printing size_t and int64_t Added definition of PRId64 for C99 compatibility. Used %zd for portable wat to print size_t. Signed-off-by: Mark Charlebois --- src/platforms/px4_defines.h | 3 +++ src/systemcmds/tests/test_bson.c | 4 ++-- src/systemcmds/tests/test_int.c | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index f85baa3b75..8660276405 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -115,6 +115,9 @@ typedef param_t px4_param_t; #ifndef PRIu64 #define PRIu64 "llu" #endif +#ifndef PRId64 +#define PRId64 "lld" +#endif /* * POSIX Specific defines diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index 6309e23160..bba1ae4f12 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -174,7 +174,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) len = bson_decoder_data_pending(decoder); if (len != strlen(sample_string) + 1) { - PX4_ERR("FAIL: decoder: string1 length %d wrong, expected %ld", len, strlen(sample_string) + 1); + PX4_ERR("FAIL: decoder: string1 length %d wrong, expected %zd", len, strlen(sample_string) + 1); return 1; } @@ -213,7 +213,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) len = bson_decoder_data_pending(decoder); if (len != sizeof(sample_data)) { - PX4_ERR("FAIL: decoder: data1 length %d, expected %lu", len, sizeof(sample_data)); + PX4_ERR("FAIL: decoder: data1 length %d, expected %zu", len, sizeof(sample_data)); return 1; } diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c index f9a24d6843..f19eff3d84 100644 --- a/src/systemcmds/tests/test_int.c +++ b/src/systemcmds/tests/test_int.c @@ -37,13 +37,13 @@ ****************************************************************************/ #include +#include #include #include #include #include -#include #include #include #include From e1de3c13c605cbc3fd2589ca5517ad81657b3c19 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 08:04:51 -0700 Subject: [PATCH 229/493] POSIX: added required header file for PRId64 Signed-off-by: Mark Charlebois --- src/systemcmds/tests/test_int.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c index f19eff3d84..01092aa2d4 100644 --- a/src/systemcmds/tests/test_int.c +++ b/src/systemcmds/tests/test_int.c @@ -36,6 +36,9 @@ * Included Files ****************************************************************************/ +#define __STDC_FORMAT_MACROS +#include + #include #include From 676303998069d16f943d0207cd7e4c40e0c930df Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 09:10:30 -0700 Subject: [PATCH 230/493] Code cleanup - Whitespace changes These are only whitespace changes Signed-off-by: Mark Charlebois --- src/modules/uORB/ORBMap.hpp | 36 +++++++++++++------ src/modules/uORB/ORBSet.hpp | 34 +++++++++++++----- src/modules/uORB/Publication.cpp | 20 ++++++----- src/modules/uORB/Publication.hpp | 21 ++++++----- src/modules/uORB/topics/mission.h | 25 +++++++------ .../uORB/topics/multirotor_motor_limits.h | 8 ++--- src/modules/uORB/uORB.cpp | 26 +++++++------- src/modules/uORB/uORB.h | 2 +- src/modules/uORB/uORBCommon.hpp | 30 ++++++++-------- src/modules/uORB/uORBCommunicator.hpp | 31 +++++++++++----- src/modules/uORB/uORBDevices_nuttx.cpp | 2 ++ src/modules/uORB/uORBDevices_posix.cpp | 2 ++ src/modules/uORB/uORBTest_UnitTest.cpp | 11 ++++++ .../uorb_unittests/uORBCommunicatorMock.hpp | 2 +- 14 files changed, 160 insertions(+), 90 deletions(-) diff --git a/src/modules/uORB/ORBMap.hpp b/src/modules/uORB/ORBMap.hpp index e50725878b..3d26735809 100644 --- a/src/modules/uORB/ORBMap.hpp +++ b/src/modules/uORB/ORBMap.hpp @@ -39,8 +39,8 @@ namespace uORB { - class DeviceNode; - class ORBMap; +class DeviceNode; +class ORBMap; } class uORB::ORBMap @@ -48,7 +48,7 @@ class uORB::ORBMap public: struct Node { struct Node *next; - const char * node_name; + const char *node_name; uORB::DeviceNode *node; }; @@ -56,9 +56,11 @@ public: _top(nullptr), _end(nullptr) { } - ~ORBMap() { + ~ORBMap() + { while (_top != nullptr) { unlinkNext(_top); + if (_top->next == nullptr) { free((void *)_top->node_name); free(_top); @@ -67,20 +69,26 @@ public: } } } - void insert(const char *node_name, uORB::DeviceNode*node) + void insert(const char *node_name, uORB::DeviceNode *node) { Node **p; - if (_top == nullptr) + + if (_top == nullptr) { p = &_top; - else + + } else { p = &_end->next; + } *p = (Node *)malloc(sizeof(Node)); - if (_end) + + if (_end) { _end = _end->next; - else { + + } else { _end = _top; } + _end->next = nullptr; _end->node_name = strdup(node_name); _end->node = node; @@ -89,34 +97,42 @@ public: bool find(const char *node_name) { Node *p = _top; + while (p) { if (strcmp(p->node_name, node_name) == 0) { return true; } + p = p->next; } + return false; } - uORB::DeviceNode* get(const char *node_name) + uORB::DeviceNode *get(const char *node_name) { Node *p = _top; + while (p) { if (strcmp(p->node_name, node_name) == 0) { return p->node; } + p = p->next; } + return nullptr; } void unlinkNext(Node *a) { Node *b = a->next; + if (b != nullptr) { if (_end == b) { _end = a; } + a->next = b->next; free((void *)b->node_name); free(b); diff --git a/src/modules/uORB/ORBSet.hpp b/src/modules/uORB/ORBSet.hpp index 8b1e0d00fb..78c58625db 100644 --- a/src/modules/uORB/ORBSet.hpp +++ b/src/modules/uORB/ORBSet.hpp @@ -38,37 +38,45 @@ class ORBSet public: struct Node { struct Node *next; - const char * node_name; + const char *node_name; }; - ORBSet() : + ORBSet() : _top(nullptr), _end(nullptr) { } - ~ORBSet() { + ~ORBSet() + { while (_top != nullptr) { unlinkNext(_top); + if (_top->next == nullptr) { free((void *)_top->node_name); free(_top); _top = nullptr; } - } + } } void insert(const char *node_name) { Node **p; - if (_top == nullptr) + + if (_top == nullptr) { p = &_top; - else + + } else { p = &_end->next; + } *p = (Node *)malloc(sizeof(Node)); - if (_end) + + if (_end) { _end = _end->next; - else { + + } else { _end = _top; } + _end->next = nullptr; _end->node_name = strdup(node_name); } @@ -76,34 +84,42 @@ public: bool find(const char *node_name) { Node *p = _top; + while (p) { if (strcmp(p->node_name, node_name) == 0) { return true; } + p = p->next; } + return false; } bool erase(const char *node_name) { Node *p = _top; + if (_top && (strcmp(_top->node_name, node_name) == 0)) { p = _top->next; free((void *)_top->node_name); free(_top); _top = p; + if (_top == nullptr) { _end = nullptr; } + return true; } + while (p->next) { if (strcmp(p->next->node_name, node_name) == 0) { unlinkNext(p); return true; } } + return nullptr; } @@ -112,10 +128,12 @@ private: void unlinkNext(Node *a) { Node *b = a->next; + if (b != nullptr) { if (_end == b) { _end = a; } + a->next = b->next; free((void *)b->node_name); free(b); diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 0ea8e5db51..bd8ecf13b4 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -51,31 +51,35 @@ #include "topics/tecs_status.h" #include "topics/rc_channels.h" -namespace uORB { +namespace uORB +{ template Publication::Publication( const struct orb_metadata *meta, - List * list) : + List *list) : T(), // initialize data structure to zero - PublicationNode(meta, list) { + PublicationNode(meta, list) +{ } template Publication::~Publication() {} template -void * Publication::getDataVoidPtr() { +void *Publication::getDataVoidPtr() +{ return (void *)(T *)(this); } PublicationNode::PublicationNode(const struct orb_metadata *meta, - List * list) : - PublicationBase(meta) { - if (list != nullptr) list->add(this); + List *list) : + PublicationBase(meta) +{ + if (list != nullptr) { list->add(this); } } - + template class __EXPORT Publication; template class __EXPORT Publication; diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 6a0c733c64..f8af00e96d 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -64,16 +64,19 @@ public: */ PublicationBase(const struct orb_metadata *meta) : _meta(meta), - _handle(nullptr) { + _handle(nullptr) + { } /** * Update the struct * @param data The uORB message struct we are updating. */ - void update(void * data) { + void update(void *data) + { if (_handle != nullptr) { orb_publish(getMeta(), getHandle(), data); + } else { setHandle(orb_advertise(getMeta(), data)); } @@ -82,7 +85,8 @@ public: /** * Deconstructor */ - virtual ~PublicationBase() { + virtual ~PublicationBase() + { } // accessors const struct orb_metadata *getMeta() { return _meta; } @@ -95,9 +99,9 @@ protected: orb_advert_t _handle; private: // forbid copy - PublicationBase(const PublicationBase&) : _meta(), _handle() {}; + PublicationBase(const PublicationBase &) : _meta(), _handle() {}; // forbid assignment - PublicationBase& operator = (const PublicationBase &); + PublicationBase &operator = (const PublicationBase &); }; /** @@ -124,7 +128,7 @@ public: * that this should be appended to. */ PublicationNode(const struct orb_metadata *meta, - List * list=nullptr); + List *list = nullptr); /** * This function is the callback for list traversal @@ -151,7 +155,7 @@ public: * list during construction */ Publication(const struct orb_metadata *meta, - List * list=nullptr); + List *list = nullptr); /** * Deconstructor @@ -170,7 +174,8 @@ public: /** * Create an update function that uses the embedded struct. */ - void update() { + void update() + { PublicationBase::update(getDataVoidPtr()); } }; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 22a8f3ecb8..226ea2e980 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -50,17 +50,17 @@ /* compatible to mavlink MAV_CMD */ enum NAV_CMD { - NAV_CMD_IDLE=0, - NAV_CMD_WAYPOINT=16, - NAV_CMD_LOITER_UNLIMITED=17, - NAV_CMD_LOITER_TURN_COUNT=18, - NAV_CMD_LOITER_TIME_LIMIT=19, - NAV_CMD_RETURN_TO_LAUNCH=20, - NAV_CMD_LAND=21, - NAV_CMD_TAKEOFF=22, - NAV_CMD_ROI=80, - NAV_CMD_PATHPLANNING=81, - NAV_CMD_DO_JUMP=177 + NAV_CMD_IDLE = 0, + NAV_CMD_WAYPOINT = 16, + NAV_CMD_LOITER_UNLIMITED = 17, + NAV_CMD_LOITER_TURN_COUNT = 18, + NAV_CMD_LOITER_TIME_LIMIT = 19, + NAV_CMD_RETURN_TO_LAUNCH = 20, + NAV_CMD_LAND = 21, + NAV_CMD_TAKEOFF = 22, + NAV_CMD_ROI = 80, + NAV_CMD_PATHPLANNING = 81, + NAV_CMD_DO_JUMP = 177 }; enum ORIGIN { @@ -102,8 +102,7 @@ struct mission_item_s { * This topic used to notify navigator about mission changes, mission itself and new mission state * must be stored in dataman before publication. */ -struct mission_s -{ +struct mission_s { int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ unsigned count; /**< count of the missions stored in the dataman */ int current_seq; /**< default -1, start at the one changed latest */ diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h index 589f8a650c..30fd96b938 100644 --- a/src/modules/uORB/topics/multirotor_motor_limits.h +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -52,10 +52,10 @@ * Motor limits */ struct multirotor_motor_limits_s { - uint8_t lower_limit : 1; // at least one actuator command has saturated on the lower limit - uint8_t upper_limit : 1; // at least one actuator command has saturated on the upper limit - uint8_t yaw : 1; // yaw limit reached - uint8_t reserved : 5; // reserved + uint8_t lower_limit : 1; // at least one actuator command has saturated on the lower limit + uint8_t upper_limit : 1; // at least one actuator command has saturated on the upper limit + uint8_t yaw : 1; // yaw limit reached + uint8_t reserved : 5; // reserved }; /** diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index c1266552fe..d292628be0 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -62,7 +62,7 @@ */ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { - return uORB::Manager::get_instance()->orb_advertise( meta, data ); + return uORB::Manager::get_instance()->orb_advertise(meta, data); } /** @@ -91,9 +91,9 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) * this function will return -1 and set errno to ENOENT. */ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - int priority) + int priority) { - return uORB::Manager::get_instance()->orb_advertise_multi( meta, data, instance, priority ); + return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority); } @@ -112,7 +112,7 @@ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *da */ int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { - return uORB::Manager::get_instance()->orb_publish( meta, handle, data ); + return uORB::Manager::get_instance()->orb_publish(meta, handle, data); } /** @@ -143,7 +143,7 @@ int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const voi */ int orb_subscribe(const struct orb_metadata *meta) { - return uORB::Manager::get_instance()->orb_subscribe( meta ); + return uORB::Manager::get_instance()->orb_subscribe(meta); } /** @@ -177,7 +177,7 @@ int orb_subscribe(const struct orb_metadata *meta) */ int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { - return uORB::Manager::get_instance()->orb_subscribe_multi( meta, instance ); + return uORB::Manager::get_instance()->orb_subscribe_multi(meta, instance); } /** @@ -188,7 +188,7 @@ int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) */ int orb_unsubscribe(int handle) { - return uORB::Manager::get_instance()->orb_unsubscribe( handle ); + return uORB::Manager::get_instance()->orb_unsubscribe(handle); } /** @@ -209,7 +209,7 @@ int orb_unsubscribe(int handle) */ int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) { - return uORB::Manager::get_instance()->orb_copy( meta, handle, buffer ); + return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer); } /** @@ -232,7 +232,7 @@ int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) */ int orb_check(int handle, bool *updated) { - return uORB::Manager::get_instance()->orb_check( handle, updated ); + return uORB::Manager::get_instance()->orb_check(handle, updated); } /** @@ -245,7 +245,7 @@ int orb_check(int handle, bool *updated) */ int orb_stat(int handle, uint64_t *time) { - return uORB::Manager::get_instance()->orb_stat( handle, time ); + return uORB::Manager::get_instance()->orb_stat(handle, time); } /** @@ -257,7 +257,7 @@ int orb_stat(int handle, uint64_t *time) */ int orb_exists(const struct orb_metadata *meta, int instance) { - return uORB::Manager::get_instance()->orb_exists( meta, instance ); + return uORB::Manager::get_instance()->orb_exists(meta, instance); } /** @@ -272,7 +272,7 @@ int orb_exists(const struct orb_metadata *meta, int instance) */ int orb_priority(int handle, int *priority) { - return uORB::Manager::get_instance()->orb_priority( handle, priority ); + return uORB::Manager::get_instance()->orb_priority(handle, priority); } /** @@ -295,6 +295,6 @@ int orb_priority(int handle, int *priority) */ int orb_set_interval(int handle, unsigned interval) { - return uORB::Manager::get_instance()->orb_set_interval( handle, interval ); + return uORB::Manager::get_instance()->orb_set_interval(handle, interval); } diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index d8e826ec8f..3ae0f56a30 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -135,7 +135,7 @@ __BEGIN_DECLS * a file-descriptor-based handle would not otherwise be in scope for the * publisher. */ -typedef void * orb_advert_t; +typedef void *orb_advert_t; /** * Advertise as the publisher of a topic. diff --git a/src/modules/uORB/uORBCommon.hpp b/src/modules/uORB/uORBCommon.hpp index 06f731e823..be2ad8a89d 100644 --- a/src/modules/uORB/uORBCommon.hpp +++ b/src/modules/uORB/uORBCommon.hpp @@ -43,23 +43,23 @@ namespace uORB { - static const unsigned orb_maxpath = 64; +static const unsigned orb_maxpath = 64; - #ifdef ERROR - # undef ERROR - #endif - /* ERROR is not defined for c++ */ - const int ERROR = -1; +#ifdef ERROR +# undef ERROR +#endif +/* ERROR is not defined for c++ */ +const int ERROR = -1; - enum Flavor { - PUBSUB, - PARAM - }; +enum Flavor { + PUBSUB, + PARAM +}; - struct orb_advertdata { - const struct orb_metadata *meta; - int *instance; - int priority; - }; +struct orb_advertdata { + const struct orb_metadata *meta; + int *instance; + int priority; +}; } #endif // _uORBCommon_hpp_ diff --git a/src/modules/uORB/uORBCommunicator.hpp b/src/modules/uORB/uORBCommunicator.hpp index e7db2e464f..971ee3ab01 100644 --- a/src/modules/uORB/uORBCommunicator.hpp +++ b/src/modules/uORB/uORBCommunicator.hpp @@ -36,10 +36,11 @@ #include + namespace uORBCommunicator { - class IChannel; - class IChannelRxHandler; +class IChannel; +class IChannelRxHandler; } /** @@ -69,7 +70,9 @@ public: * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz ) = 0; + + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz) = 0; + /** @@ -83,12 +86,14 @@ public: * Note: This does not necessarily mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t remove_subscription( const char *messageName ) = 0; + + virtual int16_t remove_subscription(const char *messageName) = 0; + /** * Register Message Handler. This is internal for the IChannel implementer* */ - virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler ) = 0; + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler) = 0; //========================================================================= @@ -109,7 +114,9 @@ public: * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t send_message( const char *messageName, int32_t length, uint8_t* data) = 0; + + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data) = 0; + }; /** @@ -132,7 +139,9 @@ public: * handler. * otherwise = failure. */ - virtual int16_t process_add_subscription( const char *messageName, int32_t msgRateInHz ) = 0; + + virtual int16_t process_add_subscription(const char *messageName, int32_t msgRateInHz) = 0; + /** * Interface to process a received control msg to remove subscription @@ -144,7 +153,9 @@ public: * handler. * otherwise = failure. */ - virtual int16_t process_remove_subscription( const char *messageName ) = 0; + + virtual int16_t process_remove_subscription(const char *messageName) = 0; + /** * Interface to process the received data message. @@ -160,7 +171,9 @@ public: * handler. * otherwise = failure. */ - virtual int16_t process_received_message( const char *messageName, int32_t length, uint8_t* data ) = 0; + + virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data) = 0; + }; #endif /* _uORBCommunicator_hpp_ */ diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index 5d58dbcae9..5d0ce3b99e 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -639,10 +639,12 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) if ((existing_node != nullptr) && !(existing_node->is_published())) { /* nothing has been published yet, lets claim it */ ret = OK; + } else { /* otherwise: data has already been published, keep looking */ } } + /* also discard the name now */ free((void *)objname); free((void *)devpath); diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 96a46beea5..7c48c4ed79 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -655,10 +655,12 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) if ((existing_node != nullptr) && !(existing_node->is_published())) { /* nothing has been published yet, lets claim it */ ret = PX4_OK; + } else { /* otherwise: data has already been published, keep looking */ } } + /* also discard the name now */ free((void *)objname); free((void *)devpath); diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORBTest_UnitTest.cpp index 7a0c15b476..6b30e75b2f 100644 --- a/src/modules/uORB/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORBTest_UnitTest.cpp @@ -141,17 +141,23 @@ int uORBTest::UnitTest::pubsublatency_main(void) int uORBTest::UnitTest::test() { int ret = test_single(); + if (ret != OK) { return ret; } + ret = test_multi(); + if (ret != OK) { return ret; } + ret = test_multi_reversed(); + if (ret != OK) { return ret; } + return OK; } @@ -323,16 +329,21 @@ int uORBTest::UnitTest::test_multi_reversed() /* Subscribe first and advertise afterwards. */ int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2); + if (sfd2 < 0) { return test_fail("sub. id2: ret: %d", sfd2); } struct orb_test t, u; + t.val = 0; + int instance2; + orb_advert_t pfd2 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX); int instance3; + orb_advert_t pfd3 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN); test_note("advertised"); diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp index e0cb3da532..8c5b861b1e 100644 --- a/unittests/uorb_unittests/uORBCommunicatorMock.hpp +++ b/unittests/uorb_unittests/uORBCommunicatorMock.hpp @@ -1,3 +1,4 @@ + /**************************************************************************** * * Copyright (c) 2015 Mark Charlebois. All rights reserved. @@ -75,7 +76,6 @@ class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel */ virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz ); - /** * @brief Interface to notify the remote entity of removal of a subscription * From 6b5a9d6c7b51a32c020b9c40dbc2248534e40518 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 09:29:05 -0700 Subject: [PATCH 231/493] QuRT: Unit tests for QuRT Signed-off-by: Mark Charlebois --- src/platforms/qurt/tests/muorb/module.mk | 7 +- .../qurt/tests/muorb/muorb_test_example.cpp | 174 +++++++++++++++++- .../qurt/tests/muorb/muorb_test_example.h | 10 + .../qurt/tests/muorb/muorb_test_main.cpp | 4 +- .../tests/muorb/muorb_test_start_qurt.cpp | 18 +- 5 files changed, 204 insertions(+), 9 deletions(-) diff --git a/src/platforms/qurt/tests/muorb/module.mk b/src/platforms/qurt/tests/muorb/module.mk index 128d894f87..e21ee3c60b 100644 --- a/src/platforms/qurt/tests/muorb/module.mk +++ b/src/platforms/qurt/tests/muorb/module.mk @@ -37,7 +37,12 @@ MODULE_COMMAND = muorb_test -SRCS = muorb_test_main.cpp \ +SRCS = \ muorb_test_start_qurt.cpp \ muorb_test_example.cpp +INCLUDE_DIRS += $(PX4_BASE)/src/modules/uORB \ + $(PX4_BASE)/src/platforms \ + $(PX4_BASE)/src/modules + + diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp index fdf46c6260..d934e1b9b7 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp @@ -42,19 +42,183 @@ #include #include #include +#include +#include "uORB/topics/sensor_combined.h" +#include "uORB/topics/pwm_input.h" +#include "uORB.h" +#include "px4_middleware.h" +#include "px4_defines.h" +#include +//#include +//#include + px4::AppState MuorbTestExample::appState; int MuorbTestExample::main() { + int rc; appState.setRunning(true); + rc = PingPongTest(); + //rc = FileReadTest(); + appState.setRunning( false ); + return rc; +} + +int MuorbTestExample::DefaultTest() +{ + struct pwm_input_s pwm; + struct sensor_combined_s sc; + //sc = new sensor_combined_s; + memset( &pwm, 0, sizeof(pwm_input_s) ); + memset( &sc, 0, sizeof(sensor_combined_s) ); + PX4_WARN( "Suucessful after memset... " ); + orb_advert_t pub_fd = orb_advertise( ORB_ID( pwm_input ), &pwm ); + if( pub_fd == nullptr ) + { + PX4_WARN( "Error: advertizing pwm_input topic" ); + return -1; + } + orb_advert_t pub_sc = orb_advertise( ORB_ID( sensor_combined ), &sc ); + if( pub_sc == nullptr ) + { + PX4_WARN( "Error: advertizing sensor_combined topic" ); + return -1; + } + int i=0; - while (!appState.exitRequested() && i<5) { + pwm.error_count++; + sc.gyro_errcount++; + //while (!appState.exitRequested() && i<5) { + while (!appState.exitRequested() && i < 10 ) { - PX4_DEBUG(" Doing work..."); - ++i; + PX4_INFO(" Doing work..."); + orb_publish( ORB_ID( pwm_input), pub_fd, &pwm ); + orb_publish( ORB_ID( sensor_combined ), pub_sc, &sc ); + //px4::usleep( 1000000 ); + //sleep( 1 ); + for( int64_t j = 0; j < 0x80; ++j ) + { + volatile int x = 0; + ++x; + } + ++i; } - - return 0; + return 0; +} + +int MuorbTestExample::PingPongTest() +{ + int i=0; + orb_advert_t pub_id_esc_status = orb_advertise( ORB_ID( esc_status ), & m_esc_status ); + if( pub_id_esc_status == 0 ) + { + PX4_ERR( "error publishing esc_status" ); + return -1; + } + if( orb_publish( ORB_ID( esc_status ), pub_id_esc_status, &m_esc_status ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the esc_status message", i ); + return -1; + } + int sub_vc = orb_subscribe( ORB_ID( vehicle_command ) ); + if ( sub_vc == PX4_ERROR ) + { + PX4_ERR( "Error subscribing to vehicle_command topic" ); + return -1; + } + + while (!appState.exitRequested() ) { + + PX4_DEBUG("[%d] Doing work...", i ); + bool updated = false; + if( orb_check( sub_vc, &updated ) == 0 ) + { + if( updated ) + { + PX4_DEBUG( "[%d]vechile command status is updated... reading new value", i ); + if( orb_copy( ORB_ID( vehicle_command ), sub_vc, &m_vc ) != 0 ) + { + PX4_ERR( "[%d]Error calling orb copy for vechicle... ", i ); + break; + } + if( orb_publish( ORB_ID( esc_status ), pub_id_esc_status, &m_esc_status ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the esc_status message", i ); + break; + } + } + else + { + PX4_DEBUG( "[%d] vechicle command topic is not updated", i ); + } + } + else + { + PX4_ERR( "[%d]Error checking the updated status for vehicle command ", i ); + break; + } + // sleep for 1 sec. + usleep( 1000000 ); + + ++i; + } + return 0; +} + +int MuorbTestExample::FileReadTest() +{ + int rc = OK; + //static const char TEST_FILE_PATH[] = "/home/linaro/test.txt"; + static const char TEST_FILE_PATH[] = "./test.txt"; + FILE* fp; + char* line = NULL; + size_t len = 0; + ssize_t read; + + fp = fopen( TEST_FILE_PATH, "r" ); + if( fp == NULL ) + { + PX4_WARN( "unable to open file[%s] for reading", TEST_FILE_PATH ); + rc = PX4_ERROR; + } + else + { + int i = 0; + //while( ( read = getline( &line, &len, fp ) ) != -1 ) + //{ + // ++i; + // PX4_WARN( "LineNum[%d] LineLength[%d]", i, len ); + // PX4_WARN( "LineNum[%d] Line[%s]", i, line ); + //} + PX4_WARN( "Successfully opened file [%s]", TEST_FILE_PATH ); + fclose( fp ); + if( line != NULL ) + { + free( line ); + } + } + +/* + std::fstream fs( TEST_FILE_PATH, std::fstream::in ); + if( fs.is_open() ) + { + int i = 0; + char line[1024]; + while( !fs.eof() ) + { + ++i; + fs.getline( line, 1024 ); + PX4_WARN( "ReadLine[%d] Line[%s]", i, line ); + } + fs.close(); + } + else + { + PX4_WARN( "Unable to open file[%s] for reading", TEST_FILE_PATH ); + rc = PX4_ERROR; + } +*/ + return rc; } diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h index 304b8464e0..5d6f70a45e 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.h +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -34,6 +34,8 @@ #pragma once #include +#include "uORB/topics/esc_status.h" +#include "uORB/topics/vehicle_command.h" class MuorbTestExample { public: @@ -44,4 +46,12 @@ public: int main(); static px4::AppState appState; /* track requests to terminate app */ +private: + int DefaultTest(); + int PingPongTest(); + int FileReadTest(); + + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc; + }; diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp index 2ded8976c6..8f8b7640e2 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp @@ -42,7 +42,9 @@ #include #include "muorb_test_example.h" -int PX4_MAIN(int argc, char **argv) +extern "C" __EXPORT int muorb_test_entry( int argc, char** argv ); + +int muorb_test_entry(int argc, char **argv) { px4::init(argc, argv, "muorb_test"); diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp index f063806766..6d64cfec8b 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -50,6 +50,18 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); +int muorb_test_entry(int argc, char **argv) +{ + //px4::init(argc, argv, "muorb_test"); + + PX4_INFO("muorb_test entry....."); + MuorbTestExample hello; + hello.main(); + + PX4_INFO("goodbye"); + return 0; +} + static void usage() { PX4_DEBUG("usage: muorb_test {start|stop|status}"); @@ -68,12 +80,14 @@ int muorb_test_main(int argc, char *argv[]) /* this is not an error */ return 0; } + + PX4_INFO( "before starting the muorb_test_entry task" ); daemon_task = px4_task_spawn_cmd("muorb_test", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, + 8192, + muorb_test_entry, (char* const*)argv); return 0; From 851a020461c99539afa0c34d6f05e9b33ee379f7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 09:48:50 -0700 Subject: [PATCH 232/493] Eagle: posix-arm and qurt changes to support Eagle HW platform The Eagle HW platform contains both a Krait (ARMv4hf compatible) cpu cluster and a Hexagon DSP running QuRT. These changes support the PX4 build for Eagle. Signed-off-by: Mark Charlebois --- Tools/qurt_apps.py | 16 ++++ makefiles/posix-arm/config_eagle_default.mk | 15 +--- makefiles/posix-arm/config_eagle_hil.mk | 86 ++++++++++++++++++ .../posix-arm/config_eagle_muorb_test.mk | 89 +++++++++++++++++++ makefiles/posix-arm/ld.script | 46 ++++++++++ .../toolchain_gnu-arm-linux-gnueabihf.mk | 13 +-- makefiles/qurt/config_qurt_hil.mk | 81 +++++++++++++++++ makefiles/qurt/config_qurt_muorb_test.mk | 79 ++++++++++++++++ makefiles/qurt/qurt_elf.mk | 6 +- makefiles/qurt/toolchain_hexagon.mk | 52 ++++++----- 10 files changed, 444 insertions(+), 39 deletions(-) create mode 100644 makefiles/posix-arm/config_eagle_hil.mk create mode 100644 makefiles/posix-arm/config_eagle_muorb_test.mk create mode 100644 makefiles/posix-arm/ld.script create mode 100644 makefiles/qurt/config_qurt_hil.mk create mode 100644 makefiles/qurt/config_qurt_muorb_test.mk diff --git a/Tools/qurt_apps.py b/Tools/qurt_apps.py index e5d75344be..b1f606c37a 100755 --- a/Tools/qurt_apps.py +++ b/Tools/qurt_apps.py @@ -48,6 +48,8 @@ print """ #include #include +#include +#include using namespace std; @@ -64,6 +66,7 @@ static int list_tasks_main(int argc, char *argv[]); static int list_files_main(int argc, char *argv[]); static int list_devices_main(int argc, char *argv[]); static int list_topics_main(int argc, char *argv[]); +static int sleep_main(int argc, char *argv[]); } @@ -78,6 +81,7 @@ print '\tapps["list_tasks"] = list_tasks_main;' print '\tapps["list_files"] = list_files_main;' print '\tapps["list_devices"] = list_devices_main;' print '\tapps["list_topics"] = list_topics_main;' +print '\tapps["sleep"] = sleep_main;' print """ } @@ -117,5 +121,17 @@ static int list_files_main(int argc, char *argv[]) px4_show_files(); return 0; } +static int sleep_main(int argc, char *argv[]) +{ + if (argc != 2) { + PX4_WARN( "Usage: sleep " ); + return 1; + } + + unsigned long usecs = ( (unsigned long) atol( argv[1] ) ) * 1000 * 1000; + PX4_WARN("Sleeping for %s, %ld",argv[1],usecs); + usleep( usecs ); + return 0; +} """ diff --git a/makefiles/posix-arm/config_eagle_default.mk b/makefiles/posix-arm/config_eagle_default.mk index d66cc5ed73..90afa155c0 100644 --- a/makefiles/posix-arm/config_eagle_default.mk +++ b/makefiles/posix-arm/config_eagle_default.mk @@ -18,7 +18,8 @@ MODULES += modules/sensors # MODULES += systemcmds/param MODULES += systemcmds/mixer -MODULES += systemcmds/topic_listener +MODULES += systemcmds/ver +#MODULES += systemcmds/topic_listener # # General system control @@ -34,7 +35,7 @@ MODULES += modules/ekf_att_pos_estimator # # Vehicle Control # -MODULES += modules/navigator +#MODULES += modules/navigator MODULES += modules/mc_pos_control MODULES += modules/mc_att_control @@ -48,7 +49,7 @@ MODULES += modules/dataman MODULES += modules/sdlog2 MODULES += modules/simulator MODULES += modules/commander -MODULES += modules/controllib +#MODULES += modules/controllib # # Libraries @@ -58,18 +59,10 @@ MODULES += lib/mathlib/math/filter MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion - # # Linux port # MODULES += platforms/posix/px4_layer -MODULES += platforms/posix/drivers/accelsim -MODULES += platforms/posix/drivers/gyrosim -MODULES += platforms/posix/drivers/adcsim -MODULES += platforms/posix/drivers/barosim -MODULES += platforms/posix/drivers/tonealrmsim -MODULES += platforms/posix/drivers/airspeedsim -MODULES += platforms/posix/drivers/gpssim # # Unit tests diff --git a/makefiles/posix-arm/config_eagle_hil.mk b/makefiles/posix-arm/config_eagle_hil.mk new file mode 100644 index 0000000000..c70ef198a8 --- /dev/null +++ b/makefiles/posix-arm/config_eagle_hil.mk @@ -0,0 +1,86 @@ +# +# Makefile for the POSIXTEST *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/rgbled +MODULES += drivers/led +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param +#MODULES += systemcmds/mixer +#MODULES += systemcmds/topic_listener +MODULES += systemcmds/ver + +# +# General system control +# +MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/navigator +#MODULES += modules/mc_pos_control +#MODULES += modules/mc_att_control + +# +# Library modules +# +MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +MODULES += modules/uORB +MODULES += modules/dataman +MODULES += modules/sdlog2 +MODULES += modules/simulator +MODULES += modules/commander +#MODULES += modules/controllib + +# +# Libraries +# +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion + +# +# Linux port +# +MODULES += platforms/posix/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim +#MODULES += platforms/posix/drivers/tonealrmsim +#MODULES += platforms/posix/drivers/airspeedsim +#MODULES += platforms/posix/drivers/gpssim + +# +# Unit tests +# +#MODULES += platforms/posix/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + +# +# muorb fastrpc changes. +# +MODULES += modules/muorb/krait diff --git a/makefiles/posix-arm/config_eagle_muorb_test.mk b/makefiles/posix-arm/config_eagle_muorb_test.mk new file mode 100644 index 0000000000..ec080c9195 --- /dev/null +++ b/makefiles/posix-arm/config_eagle_muorb_test.mk @@ -0,0 +1,89 @@ +# +# Makefile for the POSIXTEST *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/rgbled +#MODULES += drivers/led +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +#MODULES += systemcmds/param +#MODULES += systemcmds/mixer +#MODULES += systemcmds/topic_listener + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/navigator +#MODULES += modules/mc_pos_control +#MODULES += modules/mc_att_control + +# +# Library modules +# +#MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +#MODULES += modules/simulator +#MODULES += modules/commander +#MODULES += modules/controllib + +# +# Libraries +# +#MODULES += lib/mathlib +#MODULES += lib/mathlib/math/filter +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion + +# +# Linux port +# +MODULES += platforms/posix/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim +#MODULES += platforms/posix/drivers/tonealrmsim +#MODULES += platforms/posix/drivers/airspeedsim +#MODULES += platforms/posix/drivers/gpssim + +# +# muorb fastrpc changes. +# +MODULES += modules/muorb/krait + + +# +# +# Unit tests +# +#MODULES += platforms/posix/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue +MODULES += platforms/posix/tests/muorb + diff --git a/makefiles/posix-arm/ld.script b/makefiles/posix-arm/ld.script new file mode 100644 index 0000000000..32478e1e14 --- /dev/null +++ b/makefiles/posix-arm/ld.script @@ -0,0 +1,46 @@ +/**************************************************************************** + * ld.script + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Author: Mark Charlebois + * + * 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. + * + ****************************************************************************/ + +SECTIONS +{ + /* + * Construction data for parameters. + */ + __param : ALIGN(8) { + __param_start = .; + KEEP(*(__param*)) + __param_end = .; + } +} diff --git a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk index a5964e07d9..2811512ecd 100644 --- a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk +++ b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk @@ -57,6 +57,7 @@ ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) $(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) endif +EXT_MUORB_LIB_ROOT = /opt/muorb_libs # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup @@ -156,12 +157,11 @@ ARCHWARNINGS = -Wall \ -Werror=reorder \ -Werror=uninitialized \ -Werror=init-self \ - -Wno-error=logical-op \ - -Wdouble-promotion \ + -Wno-error=logical-op \ -Wlogical-op \ -Wformat=1 \ -Werror=unused-but-set-variable \ - -Werror=double-promotion \ + -Wno-error=double-promotion \ -fno-strength-reduce \ -Wno-error=unused-value @@ -188,9 +188,11 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) #EXTRA_LIBS += $(LIBM) -#EXTRA_LIBS += ${PX4_BASE}../muorb_krait/lib/libmuorb.so +EXTRA_LIBS += -lpx4muorb -ladsprpc EXTRA_LIBS += -pthread -lm -lrt +LIB_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/libs + # Flags we pass to the C compiler # CFLAGS = $(ARCHCFLAGS) \ @@ -226,7 +228,7 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ $(EXTRADEFINES) \ $(EXTRAAFLAGS) -LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script +LDSCRIPT = $(PX4_BASE)/makefiles/posix-arm/ld.script # Flags we pass to the linker # LDFLAGS += $(EXTRALDFLAGS) \ @@ -323,6 +325,7 @@ endef define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) + echo "$(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC)" $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) endef diff --git a/makefiles/qurt/config_qurt_hil.mk b/makefiles/qurt/config_qurt_hil.mk new file mode 100644 index 0000000000..9d4a48eeed --- /dev/null +++ b/makefiles/qurt/config_qurt_hil.mk @@ -0,0 +1,81 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +MODULES += drivers/hil +MODULES += drivers/led +MODULES += drivers/rgbled +MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param +MODULES += systemcmds/mixer + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +MODULES += modules/ekf_att_pos_estimator +MODULES += modules/attitude_estimator_q +MODULES += modules/position_estimator_inav + +# +# Vehicle Control +# +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +MODULES += modules/simulator +MODULES += modules/commander + +# +# Libraries +# +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion + +# +# QuRT port +# +MODULES += platforms/qurt/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim + +# +# Unit tests +# +#MODULES += platforms/qurt/tests/muorb +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + +# +# sources for muorb over fastrpc +# +MODULES += modules/muorb/adsp/ diff --git a/makefiles/qurt/config_qurt_muorb_test.mk b/makefiles/qurt/config_qurt_muorb_test.mk new file mode 100644 index 0000000000..b503e44a69 --- /dev/null +++ b/makefiles/qurt/config_qurt_muorb_test.mk @@ -0,0 +1,79 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +MODULES += drivers/hil +MODULES += drivers/led +MODULES += drivers/rgbled +MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param +MODULES += systemcmds/mixer + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +MODULES += modules/simulator +MODULES += modules/commander + +# +# Libraries +# +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion + +# +# QuRT port +# +MODULES += platforms/qurt/px4_layer +MODULES += platforms/posix/drivers/accelsim +MODULES += platforms/posix/drivers/gyrosim +MODULES += platforms/posix/drivers/adcsim +MODULES += platforms/posix/drivers/barosim + +# +# Unit tests +# +MODULES += platforms/qurt/tests/muorb +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + +# +# sources for muorb over fastrpc +# +MODULES += modules/muorb/adsp/ diff --git a/makefiles/qurt/qurt_elf.mk b/makefiles/qurt/qurt_elf.mk index 281a1603d2..73f1cd4ece 100644 --- a/makefiles/qurt/qurt_elf.mk +++ b/makefiles/qurt/qurt_elf.mk @@ -41,12 +41,12 @@ # What we're going to build. # -EXTRALDFLAGS = -Wl,-soname=libdspal_client.so +EXTRALDFLAGS = -Wl,-soname=libpx4.so PRODUCT_SHARED_LIB = $(WORK_DIR)firmware.a PRODUCT_SHARED_PRELINK = $(WORK_DIR)firmware.o .PHONY: firmware -firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)libdspal_client.so $(WORK_DIR)mainapp +firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)libpx4.so $(WORK_DIR)mainapp # # Built product rules @@ -65,7 +65,7 @@ $(WORK_DIR)apps.o: $(WORK_DIR)apps.cpp $(call COMPILEXX,$<, $@) mv $(WORK_DIR)apps.cpp $(WORK_DIR)apps.cpp_sav -$(WORK_DIR)libdspal_client.so: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB) +$(WORK_DIR)libpx4.so: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB) $(call LINK_SO,$@, $^) $(WORK_DIR)dspal_stub.o: $(PX4_BASE)/src/platforms/qurt/dspal/dspal_stub.c diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index 8b5bf5423e..d8e6427e4e 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -37,7 +37,8 @@ # Toolchain commands. Normally only used inside this file. # -HEXAGON_TOOLS_ROOT = /opt/6.4.05 +HEXAGON_TOOLS_ROOT = /opt/6.4.03 +#HEXAGON_TOOLS_ROOT = /opt/6.4.05 HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 V_ARCH = v5 CROSSDEV = hexagon- @@ -84,7 +85,8 @@ DYNAMIC_LIBS = \ # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 6.4.05 +CROSSDEV_VER_SUPPORTED = 6.4.03 +#CROSSDEV_VER_SUPPORTED = 6.4.05 CROSSDEV_VER_FOUND = $(shell $(CC) --version | sed -n 's/^.*version \([\. 0-9]*\),.*$$/\1/p') ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) @@ -94,7 +96,7 @@ endif # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup -MAXOPTIMIZATION ?= -O0 +MAXOPTIMIZATION := -O0 # Base CPU flags for each of the supported architectures. # @@ -108,31 +110,38 @@ $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -D__PX4_QURT -D__PX4_POSIX \ - -D__EXPORT= \ - -D__QDSP6_DINKUM_PTHREAD_TYPES__ \ + -D_PID_T -D_UID_T -D_TIMER_T\ -Dnoreturn_function= \ + -D__EXPORT= \ -Drestrict= \ + -D_DEBUG \ + -I$(PX4_BASE)/../dspal/include \ + -I$(PX4_BASE)/../dspal/sys \ -I$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/include \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/qurt/include \ - -I$(PX4_BASE)/../dspal/include \ - -I$(PX4_BASE)/../dspal/sys \ -I$(PX4_BASE)/mavlink/include/mavlink \ -I$(QURTLIB)/..//include \ -I$(HEXAGON_SDK_ROOT)/inc \ -I$(HEXAGON_SDK_ROOT)/inc/stddef \ -Wno-error=shadow + + # optimisation flags # -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g3 \ +ARCHOPTIMIZATION = \ + -O0 \ + -g \ -fno-strict-aliasing \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -ffunction-sections \ -fdata-sections \ - -fpic + -fpic \ + -fno-zero-initialized-in-bss + +#-fomit-frame-pointer \ +#-funsafe-math-optimizations \ +#-ffunction-sections +#$(MAXOPTIMIZATION) # enable precise stack overflow tracking # note - requires corresponding support in NuttX @@ -140,7 +149,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # -ARCHCFLAGS = -std=gnu99 +ARCHCFLAGS = -std=gnu99 -D__CUSTOM_FILE_IO__ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ # Generic warnings @@ -186,9 +195,9 @@ CFLAGS = $(ARCHCFLAGS) \ $(ARCHDEFINES) \ $(EXTRADEFINES) \ $(EXTRACFLAGS) \ - -fno-common \ $(addprefix -I,$(INCLUDE_DIRS)) + #-fno-common # Flags we pass to the C++ compiler # CXXFLAGS = $(ARCHCXXFLAGS) \ @@ -212,8 +221,7 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ LDSCRIPT = $(PX4_BASE)/makefiles/posix/ld.script # Flags we pass to the linker # -LDFLAGS += -g -mv5 -nostdlib -mG0lib -G0 -fpic -shared \ - -nostartfiles \ +LDFLAGS += -g -mv5 -mG0lib -G0 -fpic -shared \ -Wl,-Bsymbolic \ -Wl,--wrap=malloc \ -Wl,--wrap=calloc \ @@ -254,7 +262,9 @@ define COMPILE @$(ECHO) "CC: $1" @$(MKDIR) -p $(dir $2) @echo $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 - $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 + #$(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 + #$(CCACHE) $(CC) -MD -c $(CFLAGS) -D__V_DYNAMIC__ -D__FILENAME__=\"$(notdir $1)\" -fPIC $(abspath $1) -o $2 + $(CCACHE) $(CC) -c $(CFLAGS) -D__V_DYNAMIC__ -D__FILENAME__=\"$(notdir $1)\" $(abspath $1) -o $2 endef # Compile C++ source $1 to $2 for use in shared library @@ -264,7 +274,9 @@ define COMPILEXX @$(ECHO) "CXX: $1" @$(MKDIR) -p $(dir $2) @echo $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 - $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 + #$(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 + #$(CCACHE) $(CXX) -MD -c $(CXXFLAGS) -D__V_DYNAMIC__ -D__FILENAME__=\"$(notdir $1)\" -fPIC $(abspath $1) -o $2 + $(CCACHE) $(CXX) -c $(CXXFLAGS) -D__V_DYNAMIC__ -D__FILENAME__=\"$(notdir $1)\" $(abspath $1) -o $2 endef # Assemble $1 into $2 @@ -319,7 +331,7 @@ endef define LINK_SO @$(ECHO) "LINK_SO: $1" @$(MKDIR) -p $(dir $1) - $(HEXAGON_GCC) $(LDFLAGS) -fPIC -shared -nostartfiles -o $1 -Wl,--whole-archive $2 -Wl,--no-whole-archive $(LIBS) $(DYNAMIC_LIBS) + $(HEXAGON_GCC) $(LDFLAGS) -o $1 -Wl,--whole-archive $2 -Wl,--no-whole-archive $(LIBS) $(DYNAMIC_LIBS) endef # Link the objects in $2 into the application $1 From 60ec1c897a5aeb55b5613fe8fefb83343e769142 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 11:12:50 -0700 Subject: [PATCH 233/493] QuRT: Added muorb files muorb is used to proxy messages between the Krait and DSP. Signed-off-by: Mark Charlebois --- src/lib/version/version.h | 3 + src/modules/muorb/adsp/module.mk | 50 +++ src/modules/muorb/adsp/muorb_fastrpc.cpp | 157 +++++++++ src/modules/muorb/adsp/muorb_fastrpc.h | 55 +++ src/modules/muorb/adsp/uORBFastRpcChannel.cpp | 318 ++++++++++++++++++ src/modules/muorb/adsp/uORBFastRpcChannel.hpp | 251 ++++++++++++++ src/modules/muorb/krait/module.mk | 49 +++ src/modules/muorb/krait/muorb_main.cpp | 84 +++++ .../muorb/krait/uORBKraitFastRpcChannel.cpp | 190 +++++++++++ .../muorb/krait/uORBKraitFastRpcChannel.hpp | 144 ++++++++ 10 files changed, 1301 insertions(+) create mode 100644 src/modules/muorb/adsp/module.mk create mode 100644 src/modules/muorb/adsp/muorb_fastrpc.cpp create mode 100644 src/modules/muorb/adsp/muorb_fastrpc.h create mode 100644 src/modules/muorb/adsp/uORBFastRpcChannel.cpp create mode 100644 src/modules/muorb/adsp/uORBFastRpcChannel.hpp create mode 100644 src/modules/muorb/krait/module.mk create mode 100644 src/modules/muorb/krait/muorb_main.cpp create mode 100644 src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp create mode 100644 src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 9d7e471adc..b71ba1d0d9 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -62,4 +62,7 @@ #ifdef CONFIG_ARCH_BOARD_SITL #define HW_ARCH "LINUXTEST" #endif +#ifdef CONFIG_ARCH_BOARD_EAGLE +#define HW_ARCH "LINUXTEST" +#endif #endif /* VERSION_H_ */ diff --git a/src/modules/muorb/adsp/module.mk b/src/modules/muorb/adsp/module.mk new file mode 100644 index 0000000000..65d0e8e555 --- /dev/null +++ b/src/modules/muorb/adsp/module.mk @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2012-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. +# +############################################################################ + +# +# Makefile to build muorb +# + + +ifeq ($(PX4_TARGET_OS),qurt) + +SRCS = \ + muorb_fastrpc.cpp \ + uORBFastRpcChannel.cpp + +INCLUDE_DIRS += \ + ${PX4_BASE}/src/modules/uORB + +endif + +MAXOPTIMIZATION = -Os diff --git a/src/modules/muorb/adsp/muorb_fastrpc.cpp b/src/modules/muorb/adsp/muorb_fastrpc.cpp new file mode 100644 index 0000000000..febe578258 --- /dev/null +++ b/src/modules/muorb/adsp/muorb_fastrpc.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#include "muorb_fastrpc.h" +#include "qurt.h" +#include "uORBFastRpcChannel.hpp" +#include "uORBManager.hpp" + +#include +#include +#include +#include +#include +#include "px4_log.h" +#include "uORB/topics/sensor_combined.h" +#include "uORB.h" + +#define _ENABLE_MUORB 1 + +__BEGIN_DECLS + +int dspal_main(int argc, const char *argv[]); + +__END_DECLS + + +int muorb_fastrpc_orb_initialize() +{ + int rc = 0; + PX4_WARN("Before calling dspal_entry() method..."); + // registere the fastrpc muorb with uORBManager. + uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance()); + const char *argv[2] = { "dspal", "start" }; + int argc = 2; + dspal_main(argc, argv); + PX4_WARN("After calling dspal_entry"); + return rc; +} + +int muorb_fastrpc_add_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->AddRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_add_subscription(name, 0); + + if (rc != OK) { + channel->RemoveRemoteSubscriber(name); + } + + } else { + rc = -1; + } + + return rc; +} + +int muorb_fastrpc_remove_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->RemoveRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_remove_subscription(name); + + } else { + rc = -1; + } + + return rc; + +} + +int muorb_fastrpc_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_received_message(name, data_len_in_bytes, (uint8_t *)data); + + } else { + rc = -1; + } + + return rc; +} + +int muorb_fastrpc_is_subscriber_present(const char *topic_name, int *status) +{ + int rc = 0; + int32_t local_status = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->is_subscriber_present(topic_name, &local_status); + + if (rc == 0) { + *status = (int)local_status; + } + + return rc; +} + +int muorb_fastrpc_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) +{ + int rc = 0; + int32_t local_msg_type = 0; + int32_t local_bytes_returned = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, data_len_in_bytes, &local_bytes_returned); + *msg_type = (int)local_msg_type; + *bytes_returned = (int)local_bytes_returned; + return rc; +} + +int muorb_fastrpc_unblock_recieve_msg(void) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->unblock_get_data_method(); + return rc; +} diff --git a/src/modules/muorb/adsp/muorb_fastrpc.h b/src/modules/muorb/adsp/muorb_fastrpc.h new file mode 100644 index 0000000000..96eda20d35 --- /dev/null +++ b/src/modules/muorb/adsp/muorb_fastrpc.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#pragma once + +#include +#include + +__BEGIN_DECLS + +int muorb_fastrpc_orb_initialize() __EXPORT; + +int muorb_fastrpc_add_subscriber(const char *name) __EXPORT; + +int muorb_fastrpc_remove_subscriber(const char *name) __EXPORT; + +int muorb_fastrpc_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + +int muorb_fastrpc_is_subscriber_present(const char *topic_name, int *status) __EXPORT; + +int muorb_fastrpc_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) __EXPORT; + +int muorb_fastrpc_unblock_recieve_msg(void) __EXPORT; + +__END_DECLS diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp new file mode 100644 index 0000000000..87c2bba57d --- /dev/null +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -0,0 +1,318 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#include "uORBFastRpcChannel.hpp" +#include "px4_log.h" +#include + +// static intialization. +uORB::FastRpcChannel uORB::FastRpcChannel::_Instance; + +//============================================================================== +//============================================================================== +uORB::FastRpcChannel::FastRpcChannel() + : _RxHandler(0) + , _DataQInIndex(0) + , _DataQOutIndex(0) + , _ControlQInIndex(0) + , _ControlQOutIndex(0) +{ + for (int32_t i = 0; i < _MAX_MSG_QUEUE_SIZE; ++ i) { + _DataMsgQueue[i]._MaxBufferSize = 0; + _DataMsgQueue[i]._Length = 0; + _DataMsgQueue[i]._Buffer = 0; + } + + _RemoteSubscribers.clear(); +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + _Subscribers.push_back(messageName); + PX4_DEBUG("Adding message[%s] to subscriber queue...", messageName); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + _Subscribers.remove(messageName); + + return rc; +} + +int16_t uORB::FastRpcChannel::is_subscriber_present(const char *messageName, int32_t *status) +{ + int16_t rc = 0; + + if (std::find(_Subscribers.begin(), _Subscribers.end(), messageName) != _Subscribers.end()) { + *status = 1; + PX4_DEBUG("******* Found subscriber for message[%s]....", messageName); + + } else { + *status = 0; + PX4_WARN("@@@@@ Subscriber not found for[%s]...numSubscribers[%d]", messageName, _Subscribers.size()); + int i = 0; + + for (std::list::iterator it = _Subscribers.begin(); it != _Subscribers.end(); ++it) { + if (*it == messageName) { + PX4_DEBUG("##### Found the message[%s] in the subscriber list-index[%d]", messageName, i); + } + + ++i; + } + } + + return rc; +} + +int16_t uORB::FastRpcChannel::unblock_get_data_method() +{ + PX4_DEBUG("[unblock_get_data_method] calling post method for _DataAvailableSemaphore()"); + _DataAvailableSemaphore.post(); + return 0; +} +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + + if (_RemoteSubscribers.find(messageName) == _RemoteSubscribers.end()) { + //there is no-remote subscriber. So do not queue the message. + return rc; + } + + _QueueMutex.lock(); + bool overwriteData = false; + + if (IsDataQFull()) { + // queue is full. Overwrite the oldest data. + PX4_WARN("[send_message] Queue Full Overwrite the oldest data. in[%ld] out[%ld] max[%ld]", + _DataQInIndex, _DataQOutIndex, _MAX_MSG_QUEUE_SIZE); + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + overwriteData = true; + } + + // now check to see if the data queue's buffer size if large enough to memcpy the data. + // if not, delete the old buffer and re-create a new buffer of larger size. + check_and_expand_data_buffer(_DataQInIndex, length); + + // now memcpy the data to the buffer. + memcpy(_DataMsgQueue[ _DataQInIndex ]._Buffer, data, length); + _DataMsgQueue[ _DataQInIndex ]._Length = length; + _DataMsgQueue[ _DataQInIndex ]._MsgName = messageName; + + _DataQInIndex++; + + if (_DataQInIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQInIndex = 0; + } + + // the assumption here is that each caller reads only one data from either control or data queue. + if (!overwriteData) { + _DataAvailableSemaphore.post(); + } + + _QueueMutex.unlock(); + return rc; +} + +//============================================================================== +//============================================================================== +void uORB::FastRpcChannel::check_and_expand_data_buffer(int32_t index, int32_t length) +{ + if (_DataMsgQueue[ index ]._MaxBufferSize < length) { + // create a new buffer of size length and delete old buffer. + if (_DataMsgQueue[ index ]._Buffer != 0) { + delete _DataMsgQueue[ index ]._Buffer; + } + + _DataMsgQueue[ index ]._Buffer = new uint8_t[ length ]; + + if (_DataMsgQueue[ index ]._Buffer == 0) { + PX4_ERR("Error[check_and_expand_data_buffer] Failed to allocate data queue buffer of size[%ld]", length); + _DataMsgQueue[ index ]._MaxBufferSize = 0; + return; + } + + _DataMsgQueue[ index ]._MaxBufferSize = length; + } +} + +int32_t uORB::FastRpcChannel::DataQSize() +{ + int32_t rc; + rc = (_DataQInIndex - _DataQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +int32_t uORB::FastRpcChannel::ControlQSize() +{ + int32_t rc; + rc = (_ControlQInIndex - _ControlQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +bool uORB::FastRpcChannel::IsControlQFull() +{ + return (ControlQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsControlQEmpty() +{ + return (ControlQSize() == 0); +} + +bool uORB::FastRpcChannel::IsDataQFull() +{ + return (DataQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsDataQEmpty() +{ + return (DataQSize() == 0); +} + +int16_t uORB::FastRpcChannel::get_data +( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned +) +{ + int16_t rc = 0; + // wait for data availability + _DataAvailableSemaphore.wait(); + _QueueMutex.lock(); + + if (DataQSize() != 0 || ControlQSize() != 0) { + if (ControlQSize() > 0) { + // read the first element of the Control Queue. + *msg_type = _ControlMsgQueue[ _ControlQOutIndex ]._Type; + + if ((int)_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() < (int)topic_name_len) { + memcpy + ( + topic_name, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.c_str(), + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + + topic_name[_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = 0; + + _ControlQOutIndex++; + + if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _ControlQOutIndex = 0; + } + + } else { + PX4_ERR("Error[get_data-CONTROL]: max topic_name_len[%ld] < controlMsgLen[%d]", + topic_name_len, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + rc = -1; + } + + } else { + // read the first element of the Control Queue. + *msg_type = _DATA_MSG_TYPE; + + if (((int)_DataMsgQueue[ _DataQOutIndex ]._MsgName.size() < topic_name_len) || + (_DataMsgQueue[ _DataQOutIndex ]._Length < data_len_in_bytes)) { + memcpy + ( + topic_name, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.c_str(), + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + + topic_name[_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = _DataMsgQueue[ _DataQOutIndex ]._Length; + memcpy(data, _DataMsgQueue[ _DataQOutIndex ]._Buffer, _DataMsgQueue[ _DataQOutIndex ]._Length); + + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + } else { + PX4_ERR("Error:[get_data-DATA] type msg max topic_name_len[%ld] > dataMsgLen[%d] ", + topic_name_len, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + PX4_ERR("Error:[get_data-DATA] Or data_buffer_len[%ld] > message_size[%ld] ", + data_len_in_bytes, + _DataMsgQueue[ _DataQOutIndex ]._Length + ); + + rc = -1; + } + } + + } else { + PX4_ERR("[get_data] Error: Semaphore is up when there is no data on the control/data queues"); + rc = -1; + } + + _QueueMutex.unlock(); + return rc; +} diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.hpp b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp new file mode 100644 index 0000000000..0446fb1772 --- /dev/null +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp @@ -0,0 +1,251 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#ifndef _uORBFastRpcChannel_hpp_ +#define _uORBFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include +#include + +namespace uORB +{ +class FastRpcChannel; +} + +class uORB::FastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::FastRpcChannel *GetInstance() + { + return &(_Instance); + } + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + //Function to return the data to krait. + int16_t get_data + ( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned + ); + + // function to check if there are subscribers for a topic on adsp. + int16_t is_subscriber_present(const char *messageName, int32_t *status); + + // function to release the blocking semaphore for get_data method. + int16_t unblock_get_data_method(); + + uORBCommunicator::IChannelRxHandler *GetRxHandler() + { + return _RxHandler; + } + + void AddRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.insert(messageName); + } + void RemoveRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.erase(messageName); + } + +private: // data members + static uORB::FastRpcChannel _Instance; + uORBCommunicator::IChannelRxHandler *_RxHandler; + + /// data structure to store the messages to be retrived by Krait. + static const int32_t _MAX_MSG_QUEUE_SIZE = 100; + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + + struct FastRpcDataMsg { + int32_t _MaxBufferSize; + int32_t _Length; + uint8_t *_Buffer; + std::string _MsgName; + }; + + struct FastRpcControlMsg { + int32_t _Type; + std::string _MsgName; + }; + + struct FastRpcDataMsg _DataMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _DataQInIndex; + int32_t _DataQOutIndex; + + struct FastRpcControlMsg _ControlMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _ControlQInIndex; + int32_t _ControlQOutIndex; + + std::list _Subscribers; + + //utility classes + class Mutex + { + public: + Mutex() + { + sem_init(&_Sem, 0, 1); + } + ~Mutex() + { + sem_destroy(&_Sem); + } + void lock() + { + sem_wait(&_Sem); + } + void unlock() + { + sem_post(&_Sem); + } + private: + sem_t _Sem; + + Mutex(const Mutex &); + + Mutex &operator=(const Mutex &); + }; + + class Semaphore + { + public: + Semaphore() + { + sem_init(&_Sem, 0, 0); + } + ~Semaphore() + { + sem_destroy(&_Sem); + } + void post() + { + sem_post(&_Sem); + } + void wait() + { + sem_wait(&_Sem); + } + private: + sem_t _Sem; + Semaphore(const Semaphore &); + Semaphore &operator=(const Semaphore &); + + }; + + Mutex _QueueMutex; + Semaphore _DataAvailableSemaphore; + +private://class members. + /// constructor. + FastRpcChannel(); + + void check_and_expand_data_buffer(int32_t index, int32_t length); + + bool IsControlQFull(); + bool IsControlQEmpty(); + bool IsDataQFull(); + bool IsDataQEmpty(); + int32_t DataQSize(); + int32_t ControlQSize(); + + std::set _RemoteSubscribers; +}; + +#endif /* _uORBFastRpcChannel_hpp_ */ diff --git a/src/modules/muorb/krait/module.mk b/src/modules/muorb/krait/module.mk new file mode 100644 index 0000000000..c628b53f96 --- /dev/null +++ b/src/modules/muorb/krait/module.mk @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2012-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. +# +############################################################################ + +# +# Makefile to build uORB +# + +MODULE_COMMAND = muorb + +ifeq ($(PX4_TARGET_OS),posix-arm) +SRCS = uORBKraitFastRpcChannel.cpp \ + muorb_main.cpp +INCLUDE_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/include \ + $(PX4_BASE)/src/modules/uORB \ + $(PX4_BASE)/src/modules + +EXTRA_LIBS += $(EXT_MUORB_LIB_ROOT)/krait/libs/libmuorb.so +endif + diff --git a/src/modules/muorb/krait/muorb_main.cpp b/src/modules/muorb/krait/muorb_main.cpp new file mode 100644 index 0000000000..3a320e6134 --- /dev/null +++ b/src/modules/muorb/krait/muorb_main.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +#include +#include "uORBManager.hpp" +#include "uORBKraitFastRpcChannel.hpp" + +extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); } + +static void usage() +{ + warnx("Usage: muorb 'start', 'stop', 'status'"); +} + + +int +muorb_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return -EINVAL; + } + + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + // register the fast rpc channel with UORB. + uORB::Manager::get_instance()->set_uorb_communicator(uORB::KraitFastRpcChannel::GetInstance()); + + // start the KaitFastRPC channel thread. + uORB::KraitFastRpcChannel::GetInstance()->Start(); + return OK; + + } + + if (!strcmp(argv[1], "stop")) { + + uORB::KraitFastRpcChannel::GetInstance()->Stop(); + return OK; + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) { + return OK; + } + + usage(); + return -EINVAL; +} diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp new file mode 100644 index 0000000000..3900a38c94 --- /dev/null +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#include "uORBKraitFastRpcChannel.hpp" +#include "px4_log.h" + + +#define LOG_TAG "uORBKraitFastRpcChannel.cpp" + +// static intialization. +uORB::KraitFastRpcChannel uORB::KraitFastRpcChannel::_Instance; + +//============================================================================== +//============================================================================== +uORB::KraitFastRpcChannel::KraitFastRpcChannel() + : _RxHandler(nullptr) + , _ThreadStarted(false) + , _ShouldExit(false) +{ + _KraitWrapper.Initialize(); +} + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + // invoke fast_rpc call. From Idl. + PX4_DEBUG("Before calling AddSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.AddSubscriber(messageName); + PX4_DEBUG("Response for AddSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + // invoke the fast_rpc call defined in idl. + PX4_DEBUG("Before calling RemoveSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.RemoveSubscriber(messageName); + PX4_DEBUG("Response for RemoveSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + // invoke the fast rpc call to send data defined in idl. + //PX4_DEBUG( "Before calling send_data for [%s] len[%d]\n", messageName.c_str(), length ); + int32_t status = 0; + + if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) { + if (status > 0) { // there are remote subscribers + rc = _KraitWrapper.SendData(messageName, length, data); + //PX4_DEBUG( "***** SENDING[%s] topic to remote....\n", messageName.c_str() ); + + } else { + //PX4_DEBUG( "******* NO SUBSCRIBER PRESENT ON THE REMOTE FOR topic[%s] \n", messageName.c_str() ); + } + } else { + PX4_ERR("Error returned for KraitWrapper.IsSubscriberPresent(%s)\n", messageName); + } + + //PX4_DEBUG( "Response for SendMessage for [%s],len[%d] rc[%d]\n", messageName.c_str(), length, rc ); + return rc; +} + +void uORB::KraitFastRpcChannel::Start() +{ + _ThreadStarted = true; + pthread_create(&_RecvThread, NULL, thread_start, this); +} + +void uORB::KraitFastRpcChannel::Stop() +{ + _ShouldExit = true; + _KraitWrapper.UnblockReceiveData(); + PX4_DEBUG("After calling krait_wrapper_unlock_receive_Data...\n"); + pthread_join(_RecvThread, NULL); + PX4_DEBUG("*** After calling thread wait...\n"); + _ThreadStarted = false; + _ShouldExit = false; +} + + +void uORB::KraitFastRpcChannel::thread_start(void *handler) +{ + if (handler != nullptr) { + ((uORB::KraitFastRpcChannel *)handler)->fastrpc_recv_thread(); + } +} + +void uORB::KraitFastRpcChannel::fastrpc_recv_thread() +{ + // sit in while loop. + int32_t rc = 0; + int32_t type = 0; + char *name = nullptr; + int32_t data_length = 0; + uint8_t *data = nullptr; + + while (!_ShouldExit) { + // call the fastrpc recv data call. + //uorb_fastrpc_recieve( &type, &name_len, name, &data_length, data ); + rc = _KraitWrapper.ReceiveData(&type, &name, &data_length, &data); + + if (rc == 0) { + switch (type) { + case _CONTROL_MSG_TYPE_ADD_SUBSCRIBER: + if (_RxHandler != nullptr) { + _RxHandler->process_add_subscription(name, 1); + PX4_DEBUG("Received add subscriber control message for: [%s]\n", name); + } + + break; + + case _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER: + if (_RxHandler != nullptr) { + _RxHandler->process_remove_subscription(name); + PX4_DEBUG("Received remove subscriber control message for: [%s]\n", name); + } + + break; + + case _DATA_MSG_TYPE: + if (_RxHandler != nullptr) { + _RxHandler->process_received_message(name, + data_length, data); + //PX4_DEBUG( "Received topic data for control message for: [%s] len[%d]\n", name, data_length ); + } + + break; + + default: + // error condition. + break; + } + + } else { + PX4_DEBUG("Error: Getting data over fastRPC channel\n"); + break; + } + } + + PX4_DEBUG("[uORB::KraitFastRpcChannel::fastrpc_recv_thread] Exiting fastrpc_recv_thread\n"); +} + diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp new file mode 100644 index 0000000000..5c61f1e961 --- /dev/null +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#ifndef _uORBKraitFastRpcChannel_hpp_ +#define _uORBKraitFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include "muorbKraitFastRpcWrapper.hpp" + +namespace uORB +{ +class KraitFastRpcChannel; +} + +class uORB::KraitFastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::KraitFastRpcChannel *GetInstance() + { + return &(_Instance); + } + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + + void Start(); + void Stop(); + +private: // data members + static uORB::KraitFastRpcChannel _Instance; + uORBCommunicator::IChannelRxHandler *_RxHandler; + pthread_t _RecvThread; + bool _ThreadStarted; + bool _ShouldExit; + + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + + muorb::KraitFastRpcWrapper _KraitWrapper; + + + +private://class members. + /// constructor. + KraitFastRpcChannel(); + + static void thread_start(void *handler); + + void fastrpc_recv_thread(); + +}; + +#endif /* _uORBKraitFastRpcChannel_hpp_ */ From b5e6111d7c24990c37d3e4127da7c3027ce8112d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 12:54:27 -0700 Subject: [PATCH 234/493] QuRT: src/platform/qurt changes Changes to support QuRT intrgration with DSPAL and move from simulator to real HW. Signed-off-by: Mark Charlebois --- src/platforms/qurt/include/hrt_work.h | 10 +- src/platforms/qurt/px4_layer/commands_hil.c | 107 ++++++++++++++++++ .../qurt/px4_layer/commands_muorb_test.c | 33 ++++++ src/platforms/qurt/px4_layer/drv_hrt.c | 6 + .../qurt/px4_layer/hrt_work_cancel.c | 1 + src/platforms/qurt/px4_layer/main.cpp | 79 +++++++++++-- src/platforms/qurt/px4_layer/module.mk | 12 +- .../qurt/px4_layer/px4_qurt_impl.cpp | 16 ++- .../qurt/px4_layer/px4_qurt_tasks.cpp | 42 ++++++- src/platforms/qurt/px4_layer/qurt_stubs.c | 42 ++++++- 10 files changed, 320 insertions(+), 28 deletions(-) create mode 100644 src/platforms/qurt/px4_layer/commands_hil.c diff --git a/src/platforms/qurt/include/hrt_work.h b/src/platforms/qurt/include/hrt_work.h index 92b079ac6b..3dddfa7f5b 100644 --- a/src/platforms/qurt/include/hrt_work.h +++ b/src/platforms/qurt/include/hrt_work.h @@ -1,3 +1,4 @@ + /**************************************************************************** * * Copyright (C) 2015 Mark Charlebois. All rights reserved. @@ -31,6 +32,7 @@ * ****************************************************************************/ + #include #include #include @@ -46,16 +48,16 @@ void hrt_work_queue_init(void); int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); void hrt_work_cancel(struct work_s *work); -inline void hrt_work_lock(void); -inline void hrt_work_unlock(void); +static inline void hrt_work_lock(void); +static inline void hrt_work_unlock(void); -inline void hrt_work_lock() +static inline void hrt_work_lock() { //PX4_INFO("hrt_work_lock"); sem_wait(&_hrt_work_lock); } -inline void hrt_work_unlock() +static inline void hrt_work_unlock() { //PX4_INFO("hrt_work_unlock"); sem_post(&_hrt_work_lock); diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c new file mode 100644 index 0000000000..b795fba341 --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file commands_muorb_test.c + * Commands to run for the "qurt_muorb_test" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + + static const char *commands = + "uorb start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" +// "rgbled start\n" +// "tone_alarm start\n" + "commander start\n" + "sensors start\n" + //"ekf_att_pos_estimator start\n" + "attitude_estimator_q start\n" + "position_estimator_inav start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "sleep 1\n" + "hil mode_pwm\n" + "param set RC1_MAX 2015\n" + "param set RC1_MIN 996\n" + "param set RC1_TRIM 1502\n" + "param set RC1_REV -1\n" + "param set RC2_MAX 2016 \n" + "param set RC2_MIN 995\n" + "param set RC2_TRIM 1500\n" + "param set RC3_MAX 2003\n" + "param set RC3_MIN 992\n" + "param set RC3_TRIM 992\n" + "param set RC4_MAX 2011\n" + "param set RC4_MIN 997\n" + "param set RC4_TRIM 1504\n" + "param set RC4_REV -1\n" + "param set RC6_MAX 2016\n" + "param set RC6_MIN 992\n" + "param set RC6_TRIM 1504\n" + "param set RC_CHAN_CNT 8\n" + "param set RC_MAP_MODE_SW 5\n" + "param set RC_MAP_POSCTL_SW 7\n" + "param set RC_MAP_RETURN_SW 8\n" + "param set MC_YAW_P 1.5\n" + "param set MC_PITCH_P 3.0\n" + "param set MC_ROLL_P 3.0\n" + "param set MC_YAWRATE_P 0.2\n" + "param set MC_PITCHRATE_P 0.03\n" + "param set MC_ROLLRATE_P 0.03\n" + "param set ATT_W_ACC 0.0002\n" + "param set ATT_W_MAG 0.002\n" + "param set ATT_W_GYRO_BIAS 0.05\n" + "sleep 1\n" + + + "param set MAV_TYPE 2\n" + "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n" + "list_devices\n" + "list_files\n" + "list_tasks\n" + "list_topics\n" + "sleep 10\n" + "list_tasks\n" + "sleep 10\n" + + ; + + return commands; + +} diff --git a/src/platforms/qurt/px4_layer/commands_muorb_test.c b/src/platforms/qurt/px4_layer/commands_muorb_test.c index e594b9dad8..27318af9c6 100644 --- a/src/platforms/qurt/px4_layer/commands_muorb_test.c +++ b/src/platforms/qurt/px4_layer/commands_muorb_test.c @@ -43,5 +43,38 @@ const char *get_commands() "uorb start\n" "muorb_test start\n"; +/* + "hil mode_pwm\n" + "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"; +*/ +/* + "param show\n" + "param set CAL_GYRO_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "gyrosim start\n" + "accelsim start\n" + "rgbled start\n" + "tone_alarm start\n" + "simulator start -s\n" + "commander start\n" + "sensors start\n" + "ekf_att_pos_estimator start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "param set MAV_TYPE 2\n" + "param set RC1_MAX 2015\n" + "param set RC1_MIN 996\n" + "param set RC_TRIM 1502\n" +*/ + return commands; +/*====================================== Working set +======================================*/ + + //"muorb_test start\n" + //"gyrosim start\n" + //"adcsim start\n" + } diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 3edf34f64c..916992cae4 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -46,6 +46,8 @@ static struct sq_queue_s callout_queue; +extern uint64_t get_abs_time_in_us(); + /* latency histogram */ #define LATENCY_BUCKET_COUNT 8 __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; @@ -81,11 +83,15 @@ static void hrt_unlock(void) */ hrt_abstime hrt_absolute_time(void) { + + return get_abs_time_in_us(); +/* struct timespec ts; // FIXME - clock_gettime unsupported in QuRT //clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); +*/ } /* diff --git a/src/platforms/qurt/px4_layer/hrt_work_cancel.c b/src/platforms/qurt/px4_layer/hrt_work_cancel.c index c1c2c3bef7..864f4b695f 100644 --- a/src/platforms/qurt/px4_layer/hrt_work_cancel.c +++ b/src/platforms/qurt/px4_layer/hrt_work_cancel.c @@ -40,6 +40,7 @@ #include #include #include +#include #include #include "hrt_work.h" diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 864f1d22c2..e34757000a 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ using namespace std; extern void init_app_map(map &apps); extern void list_builtins(map &apps); +static px4_task_t g_dspal_task = -1; __BEGIN_DECLS // The commands to run are specified in a target file: commands_.c @@ -65,14 +67,16 @@ static void run_cmd(map &apps, const vector &appargs) unsigned int i = 0; while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); - //printf(" arg = '%s'\n", arg[i]); + PX4_WARN(" arg = '%s'\n", arg[i]); ++i; } arg[i] = (char *)0; + //PX4_DEBUG_PRINTF(i); apps[command](i,(char **)arg); } else { + PX4_WARN("NOT FOUND."); list_builtins(apps); } } @@ -91,10 +95,16 @@ static void process_commands(map &apps, const char *cmds) int i=0; const char *b = cmds; bool found_first_char = false; - char arg[20]; + char arg[256]; + + // This is added because it is a parameter used by commander, yet created by mavlink. Since mavlink is not + // running on QURT, we need to manually define it so it is available to commander. "2" is for quadrotor. + + PARAM_DEFINE_INT32(MAV_TYPE,2); // Eat leading whitespace eat_whitespace(b, i); + for(;;) { // End of command line @@ -105,7 +115,10 @@ static void process_commands(map &apps, const char *cmds) // If we have a command to run if (appargs.size() > 0) { - run_cmd(apps, appargs); + PX4_WARN("Processing command: %s",appargs[0].c_str()); + for(int ai=1;ai<(int)appargs.size();ai++) + PX4_WARN(" > arg: %s",appargs[ai].c_str()); + run_cmd(apps, appargs); } appargs.clear(); if (b[i] == '\n') { @@ -133,17 +146,63 @@ extern void init_once(void); }; __BEGIN_DECLS -void dspal_entry() -{ - const char *argv[2] = { "dspal_client", NULL }; - int argc = 1; +extern int dspal_main(int argc, char *argv[]); +__END_DECLS - printf("In main\n"); + +int dspal_entry( int argc, char* argv[] ) +{ + //const char *argv[2] = { "dspal_client", NULL }; + //int argc = 1; + + PX4_INFO("In main\n"); map apps; init_app_map(apps); px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); - for (;;) { sleep(100000); } + for( ;; ) + { + volatile int x = 0; + ++x; + } + return 0; +} + +static void usage() +{ + PX4_WARN("Usage: dspal {start |stop}"); +} + + +extern "C" { + +int dspal_main(int argc, char *argv[]) +{ + int ret = 0; + if (argc == 2 && strcmp(argv[1], "start") == 0) { + g_dspal_task = px4_task_spawn_cmd("dspal", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + dspal_entry, + argv); + + } + else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_dspal_task < 0) { + PX4_WARN("start up thread not running"); + } + else { + px4_task_delete(g_dspal_task); + g_dspal_task = -1; + } + } + else { + usage(); + ret = -1; + } + + return ret; +} } -__END_DECLS diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 13b41db3ad..2747c18b8b 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -35,6 +35,8 @@ # NuttX / uORB adapter library # +MODULE_NAME = dspal + SRCDIR=$(dir $(MODULE_MK)) SRCS = \ @@ -56,8 +58,10 @@ SRCS = \ sq_remfirst.c \ sq_addafter.c \ dq_rem.c \ - main.cpp \ - qurt_stubs.c + hrt_work.c \ + qurt_stubs.c \ + qurt_hacks.c \ + main.cpp ifeq ($(CONFIG),qurt_hello) SRCS += commands_hello.c endif @@ -67,5 +71,9 @@ endif ifeq ($(CONFIG),qurt_muorb_test) SRCS += commands_muorb_test.c endif +ifeq ($(CONFIG),qurt_hil) +SRCS += commands_hil.c +endif + MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index a5782a6f25..fcae64f8c5 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -50,14 +50,18 @@ #include #include "systemlib/param/param.h" #include "hrt_work.h" +#include "px4_log.h" -extern pthread_t _shell_task_id; +//extern pthread_t _shell_task_id; + __BEGIN_DECLS +extern uint64_t get_ticks_per_us(); // FIXME - sysconf(_SC_CLK_TCK) not supported -long PX4_TICKS_PER_SEC = 1000; +//long PX4_TICKS_PER_SEC = get_ticks_per_us(); +long PX4_TICKS_PER_SEC = 800000000; unsigned int sleep(unsigned int sec) { @@ -94,12 +98,16 @@ void init_once(void) { // Required for QuRT //_posix_init(); + PX4_WARN( "Before calling work_queue_init" ); + +// _shell_task_id = pthread_self(); +// PX4_INFO("Shell id is %lu", _shell_task_id); - _shell_task_id = pthread_self(); - PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); + PX4_WARN( "Before calling hrt_init" ); hrt_work_queue_init(); hrt_init(); + PX4_WARN( "after calling hrt_init" ); } void init(int argc, char *argv[], const char *app_name) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index cf6d665724..48c1e5f252 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -43,7 +43,11 @@ #include #include #include + +#if !defined(__PX4_QURT) #include +#endif + #include #include #include @@ -86,6 +90,12 @@ static void *entry_adapter ( void *ptr ) data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); + PX4_WARN( "Before waiting infinte busy loop" ); + //for( ;; ) + //{ + // volatile int x = 0; + // ++x; + // } free(ptr); px4_task_exit(0); @@ -157,7 +167,17 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int return (rv < 0) ? rv : -rv; } #endif + size_t fixed_stacksize = -1; + pthread_attr_getstacksize(&attr, &fixed_stacksize); + PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size ); + fixed_stacksize = 8 * 1024; + fixed_stacksize = ( fixed_stacksize < (size_t)stack_size )? (size_t)stack_size:fixed_stacksize; + PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize ); + pthread_attr_setstacksize(&attr, fixed_stacksize); + //pthread_attr_setstacksize(&attr, stack_size); + + param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); @@ -236,7 +256,7 @@ int px4_task_kill(px4_task_t id, int sig) { int rv = 0; pthread_t pid; - PX4_DEBUG("Called px4_task_kill %d", sig); + PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str()); if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) pid = taskmap[id].pid; @@ -272,9 +292,9 @@ __BEGIN_DECLS int px4_getpid() { pthread_t pid = pthread_self(); - - if (pid == _shell_task_id) - return SHELL_TASK_ID; +// +// if (pid == _shell_task_id) +// return SHELL_TASK_ID; // Get pthread ID from the opaque ID for (int i=0; i Date: Wed, 1 Jul 2015 11:05:45 -0700 Subject: [PATCH 235/493] POSIX: fixes for use of open vs px4_open, etc Fixes for the posix build when virtual devices are used. Signed-off-by: Mark Charlebois --- src/modules/commander/PreflightCheck.cpp | 8 +- .../commander/calibration_routines.cpp | 2 +- src/modules/commander/commander.cpp | 41 +++--- src/modules/commander/mag_calibration.cpp | 8 +- .../commander/state_machine_helper_posix.cpp | 2 +- .../ekf_att_pos_estimator_main.cpp | 125 +++++++++--------- .../position_estimator_inav_main.c | 2 +- src/systemcmds/tests/test_int.c | 1 + 8 files changed, 94 insertions(+), 95 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 9d38e8179a..1a9d0ad57b 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -268,7 +268,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional) } out: - close(fd); + px4_close(fd); return success; } @@ -279,10 +279,10 @@ static bool gnssCheck(int mavlink_fd) int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); //Wait up to 2000ms to allow the driver to detect a GNSS receiver module - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = gpsSub; fds[0].events = POLLIN; - if(poll(fds, 1, 2000) <= 0) { + if(px4_poll(fds, 1, 2000) <= 0) { success = false; } else { @@ -298,7 +298,7 @@ static bool gnssCheck(int mavlink_fd) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); } - close(gpsSub); + px4_close(gpsSub); return success; } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 2afc27c14b..fedafac6cd 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -493,7 +493,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, } if (sub_accel >= 0) { - close(sub_accel); + px4_close(sub_accel); } return result; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a794d203be..db6e8aeb1d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -362,7 +362,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); int checkres = prearm_check(&status, mavlink_fd_local); - close(mavlink_fd_local); + px4_close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -371,14 +371,14 @@ int commander_main(int argc, char *argv[]) int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); arm_disarm(true, mavlink_fd_local, "command line"); warnx("note: not updating home position on commandline arming!"); - close(mavlink_fd_local); + px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "disarm")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); arm_disarm(false, mavlink_fd_local, "command line"); - close(mavlink_fd_local); + px4_close(mavlink_fd_local); return 0; } @@ -389,10 +389,10 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { if (reason) { - fprintf(stderr, "%s\n", reason); + PX4_INFO("%s\n", reason); } - fprintf(stderr, "usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); + PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); } void print_status() @@ -444,7 +444,7 @@ void print_status() break; } - close(state_sub); + px4_close(state_sub); warnx("arming: %s", armed_str); @@ -931,7 +931,6 @@ int commander_thread_main(int argc, char *argv[]) if (battery_init() != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL"); } - mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* vehicle status topic */ @@ -2166,7 +2165,7 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } - fflush(stdout); + //fflush(stdout); counter++; int blink_state = blink_msg_state(); @@ -2200,18 +2199,18 @@ int commander_thread_main(int argc, char *argv[]) /* close fds */ led_deinit(); buzzer_deinit(); - close(sp_man_sub); - close(offboard_control_mode_sub); - close(local_position_sub); - close(global_position_sub); - close(gps_sub); - close(sensor_sub); - close(safety_sub); - close(cmd_sub); - close(subsys_sub); - close(diff_pres_sub); - close(param_changed_sub); - close(battery_sub); + px4_close(sp_man_sub); + px4_close(offboard_control_mode_sub); + px4_close(local_position_sub); + px4_close(global_position_sub); + px4_close(gps_sub); + px4_close(sensor_sub); + px4_close(safety_sub); + px4_close(cmd_sub); + px4_close(subsys_sub); + px4_close(diff_pres_sub); + px4_close(param_changed_sub); + px4_close(battery_sub); thread_running = false; @@ -2977,7 +2976,7 @@ void *commander_low_prio_loop(void *arg) } } - close(cmd_sub); + px4_close(cmd_sub); return NULL; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 8e11f3e65e..1a6977eb1e 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -243,7 +243,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; - close(sub_gyro); + px4_close(sub_gyro); return result; } @@ -256,12 +256,12 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } /* Wait clocking for new data on all gyro */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = sub_gyro; fds[0].events = POLLIN; size_t fd_count = 1; - int poll_ret = poll(fds, fd_count, 1000); + int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { struct gyro_report gyro; @@ -281,7 +281,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } } - close(sub_gyro); + px4_close(sub_gyro); uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; unsigned poll_errcount = 0; diff --git a/src/modules/commander/state_machine_helper_posix.cpp b/src/modules/commander/state_machine_helper_posix.cpp index 2d3b78ddaf..000cbf366a 100644 --- a/src/modules/commander/state_machine_helper_posix.cpp +++ b/src/modules/commander/state_machine_helper_posix.cpp @@ -371,7 +371,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, break; /* skip mavlink */ - if (!strcmp("/dev/mavlink", devname)) { + if (!strcmp(MAVLINK_LOG_DEVICE, devname)) { continue; } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5aba1d014e..6a8ec46053 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -73,8 +73,8 @@ static uint64_t IMUusec = 0; //Constants static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds -static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds -static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure +static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds +static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) @@ -246,10 +246,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : if (fd >= 0) { res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]); - close(fd); + px4_close(fd); if (res) { - warnx("G%u SCALE FAIL", s); + PX4_WARN("G%u SCALE FAIL", s); } } @@ -261,7 +261,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : px4_close(fd); if (res) { - warnx("A%u SCALE FAIL", s); + PX4_WARN("A%u SCALE FAIL", s); } } @@ -273,7 +273,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : px4_close(fd); if (res) { - warnx("M%u SCALE FAIL", s); + PX4_WARN("M%u SCALE FAIL", s); } } } @@ -404,7 +404,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Do not warn about accel offset if we have no position updates if (!(warn_index == 5 && _ekf->staticMode)) { - warnx("reset: %s", feedback[warn_index]); + PX4_WARN("reset: %s", feedback[warn_index]); mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); } } @@ -461,7 +461,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (_debug > 10) { if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { - warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", + PX4_INFO("health: VEL:%s POS:%s HGT:%s OFFS:%s", ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), @@ -469,7 +469,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() } if (rep.timeout_flags) { - warnx("timeout: %s%s%s%s", + PX4_INFO("timeout: %s%s%s%s", ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), @@ -515,13 +515,13 @@ void AttitudePositionEstimatorEKF::task_main() _ekf = new AttPosEKF(); - _filter_start_time = hrt_absolute_time(); - if (!_ekf) { - warnx("OUT OF MEM!"); + PX4_ERR("OUT OF MEM!"); return; } + _filter_start_time = hrt_absolute_time(); + /* * do subscriptions */ @@ -659,7 +659,7 @@ void AttitudePositionEstimatorEKF::task_main() // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); // perf_reset(_perf_baro); -// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", +// PX4_INFO("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", // (double)_baro_gps_offset, // (double)_baro_alt_filt, // (double)_gps_alt_filt, @@ -683,7 +683,7 @@ void AttitudePositionEstimatorEKF::task_main() _filter_ref_offset = -_baro.altitude; - warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); + PX4_INFO("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); } else { @@ -794,11 +794,11 @@ void AttitudePositionEstimatorEKF::initializeGPS() initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); #if 0 - warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, + PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, + PX4_INFO("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_filter_ref_offset); - warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, + PX4_INFO("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif @@ -1132,7 +1132,7 @@ void AttitudePositionEstimatorEKF::print_status() math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", + PX4_INFO("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); // State vector: @@ -1145,43 +1145,43 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); - printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt); - printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt); - printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_ref_offset, + PX4_INFO("dtIMU: %8.6f filt: %8.6f IMUmsec: %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); + PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt); + PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt); + PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset, (double)_baro_gps_offset); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, + PX4_INFO("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, + PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], + PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], + PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], + PX4_INFO("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], + PX4_INFO("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); if (EKF_STATE_ESTIMATES == 23) { - printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], + PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); + PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); + PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], + PX4_INFO("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); - printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); + PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]); } else { - printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], + PX4_INFO("states (wind) [13-14]: %8.4f, %8.4f", (double)_ekf->states[13], (double)_ekf->states[14]); + PX4_INFO("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], + PX4_INFO("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); } - printf("states: %s %s %s %s %s %s %s %s %s %s\n", + PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", @@ -1324,7 +1324,7 @@ void AttitudePositionEstimatorEKF::pollData() last_mag = _sensor_combined.magnetometer_timestamp; - //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + //PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); //Update Land Detector bool newLandData; @@ -1413,21 +1413,21 @@ void AttitudePositionEstimatorEKF::pollData() } } - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); + //PX4_INFO("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { - // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { - // _ekf->vneSigma = _parameters.velne_noise; + // _ekf->vneSigma = _parameters.velne_noise; // } // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { - // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); // } else { - // _ekf->posNeSigma = _parameters.posne_noise; + // _ekf->posNeSigma = _parameters.posne_noise; // } - // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + // PX4_INFO("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); _previousGPSTimestamp = _gps.timestamp_position; @@ -1572,42 +1572,42 @@ int AttitudePositionEstimatorEKF::trip_nan() // If system is not armed, inject a NaN value into the filter if (_armed.armed) { - warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); + PX4_INFO("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); ret = 1; } else { float nan_val = 0.0f / 0.0f; - warnx("system not armed, tripping state vector with NaN"); + PX4_INFO("system not armed, tripping state vector with NaN"); _ekf->states[5] = nan_val; usleep(100000); - warnx("tripping covariance #1 with NaN"); + PX4_INFO("tripping covariance #1 with NaN"); _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #2 with NaN"); + PX4_INFO("tripping covariance #2 with NaN"); _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #3 with NaN"); + PX4_INFO("tripping covariance #3 with NaN"); _ekf->P[3][3] = nan_val; // covariance matrix usleep(100000); - warnx("tripping Kalman gains with NaN"); + PX4_INFO("tripping Kalman gains with NaN"); _ekf->Kfusion[0] = nan_val; // Kalman gains usleep(100000); - warnx("tripping stored states[0] with NaN"); + PX4_INFO("tripping stored states[0] with NaN"); _ekf->storedStates[0][0] = nan_val; usleep(100000); - warnx("tripping states[9] with NaN"); + PX4_INFO("tripping states[9] with NaN"); _ekf->states[9] = nan_val; usleep(100000); - warnx("\nDONE - FILTER STATE:"); + PX4_INFO("DONE - FILTER STATE:"); print_status(); } @@ -1617,45 +1617,44 @@ int AttitudePositionEstimatorEKF::trip_nan() int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 2) { - warnx("usage: ekf_att_pos_estimator {start|stop|status|logging}"); + PX4_ERR("usage: ekf_att_pos_estimator {start|stop|status|logging}"); return 1; } if (!strcmp(argv[1], "start")) { if (estimator::g_estimator != nullptr) { - warnx("already running"); + PX4_ERR("already running"); return 1; } estimator::g_estimator = new AttitudePositionEstimatorEKF(); if (estimator::g_estimator == nullptr) { - warnx("alloc failed"); + PX4_ERR("alloc failed"); return 1; } if (OK != estimator::g_estimator->start()) { delete estimator::g_estimator; estimator::g_estimator = nullptr; - warnx("start failed"); + PX4_ERR("start failed"); return 1; } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) { usleep(50000); - printf("."); - fflush(stdout); + PX4_INFO("."); } - printf("\n"); + PX4_INFO(" "); return 0; } if (estimator::g_estimator == nullptr) { - warnx("not running"); + PX4_ERR("not running"); return 1; } @@ -1667,7 +1666,7 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } if (!strcmp(argv[1], "status")) { - warnx("running"); + PX4_INFO("running"); estimator::g_estimator->print_status(); @@ -1693,6 +1692,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) return ret; } - warnx("unrecognized command"); + PX4_ERR("unrecognized command"); return 1; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 7683daed2c..158d2ef27d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -148,7 +148,7 @@ int position_estimator_inav_main(int argc, char *argv[]) inav_verbose_mode = false; - if (argc > 2 && !strcmp(argv[2], "-v")) { + if ((argc > 2) && (!strcmp(argv[2], "-v"))) { inav_verbose_mode = true; } diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c index 01092aa2d4..051a48e6c2 100644 --- a/src/systemcmds/tests/test_int.c +++ b/src/systemcmds/tests/test_int.c @@ -57,6 +57,7 @@ #include #include +#include /**************************************************************************** From 347e3e9a7ebb16402f57c16b055a058fd607b7c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 00:37:39 +0200 Subject: [PATCH 236/493] PX4 log header: Add missing include --- src/platforms/px4_log.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 327df7abe1..21f17ce68e 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -54,6 +54,7 @@ #include #include #include +#include __BEGIN_DECLS __EXPORT extern uint64_t hrt_absolute_time(void); From 0e7fab457b81cd055c3effc82302ae4dabec9c08 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 15:59:20 -0700 Subject: [PATCH 237/493] Removed extra whitespace Signed-off-by: Mark Charlebois --- src/platforms/qurt/include/hrt_work.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/platforms/qurt/include/hrt_work.h b/src/platforms/qurt/include/hrt_work.h index 3dddfa7f5b..019056501a 100644 --- a/src/platforms/qurt/include/hrt_work.h +++ b/src/platforms/qurt/include/hrt_work.h @@ -1,4 +1,3 @@ - /**************************************************************************** * * Copyright (C) 2015 Mark Charlebois. All rights reserved. @@ -32,7 +31,6 @@ * ****************************************************************************/ - #include #include #include From 043bf9a4d774625645071ed832e152a8a821dd7f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 16:13:49 -0700 Subject: [PATCH 238/493] Chage use of fabsf for int to abs Use of fabsf() for int arg failed for clang. Changed to use abs(). Signed-off-by: Mark Charlebois --- src/systemcmds/tests/test_mixer.cpp | 4 ++-- src/systemcmds/tests/test_ppm_loopback.c | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index d0f76d3b69..672d9cd45a 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -259,7 +259,7 @@ int test_mixer(int argc, char *argv[]) for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; - if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + if (abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; @@ -333,7 +333,7 @@ int test_mixer(int argc, char *argv[]) /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && - fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + abs(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index 7f1323f2b9..2e884f9c6c 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -162,7 +162,7 @@ int test_ppm_loopback(int argc, char *argv[]) /* go and check values */ for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) { + if (abs(rc_input.values[i] - pwm_values[i]) > 10) { warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]); (void)close(servo_fd); return ERROR; From f659a3e8cc2ff4216f69ac1c3810c9f84c056d5e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 16:55:20 -0700 Subject: [PATCH 239/493] POSIX: do not error on stack size warning posix build fails on x86_64 with this check enabled. Signed-off-by: Mark Charlebois --- src/systemcmds/tests/module.mk | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index ff4d07f57b..1c5d266a75 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -36,9 +36,13 @@ SRCS = test_adc.c \ ifeq ($(PX4_TARGET_OS), nuttx) SRCS += test_time.c + +EXTRACXXFLAGS = -Wframe-larger-than=2500 +else +EXTRACXXFLAGS = endif -EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal +EXTRACXXFLAGS += -Wno-float-equal # Flag is only valid for GCC, not clang ifneq ($(USE_GCC), 0) From 381b889526d05567fbba0140b6499cf67defa063 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 18:00:49 -0700 Subject: [PATCH 240/493] POSIX: don't check stack size for position_estimator_inav posix build fails on x86_64 with this check enabled. Signed-off-by: Mark Charlebois --- src/modules/position_estimator_inav/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 56aa3fad04..57b32954c5 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -42,5 +42,7 @@ SRCS = position_estimator_inav_main.c \ MODULE_STACKSIZE = 1200 +ifeq ($(PX4_TARGEGT_OS),nuttx) EXTRACFLAGS = -Wframe-larger-than=3800 +endif From c611749b4fe6a46b532dbe19f9512fd720a1f3a6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 11:38:19 -0700 Subject: [PATCH 241/493] Simulator: modified -p to publish individual sensor data The simulator was changed to publish the sensor data that is read by the sensors module when the -p flag is passed. Signed-off-by: Mark Charlebois --- msg/hil_sensor.msg | 100 ++++++++++ src/modules/simulator/simulator.cpp | 72 +------ src/modules/simulator/simulator.h | 51 ++--- src/modules/simulator/simulator_mavlink.cpp | 205 +++++++++++++++++--- 4 files changed, 298 insertions(+), 130 deletions(-) create mode 100644 msg/hil_sensor.msg diff --git a/msg/hil_sensor.msg b/msg/hil_sensor.msg new file mode 100644 index 0000000000..9317722db4 --- /dev/null +++ b/msg/hil_sensor.msg @@ -0,0 +1,100 @@ +# Definition of the hil_sensor uORB topic. + +int32 MAGNETOMETER_MODE_NORMAL = 0 +int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1 +int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 + +# Sensor readings in raw and SI-unit form. +# +# These values are read from the sensors. Raw values are in sensor-specific units, +# the scaled values are in SI-units, as visible from the ending of the variable +# or the comments. The use of the SI fields is in general advised, as these fields +# are scaled and offset-compensated where possible and do not change with board +# revisions and sensor updates. +# +# Actual data, this is specific to the type of data which is stored in this struct +# A line containing L0GME will be added by the Python logging code generator to the logged dataset. +# +# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only + +uint64 timestamp # Timestamp in microseconds since boot, from gyro +# +int16[3] gyro_raw # Raw sensor values of angular velocity +float32[3] gyro_rad_s # Angular velocity in radian per seconds +uint32 gyro_errcount # Error counter for gyro 0 +float32 gyro_temp # Temperature of gyro 0 + +int16[3] accelerometer_raw # Raw acceleration in NED body frame +float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 +int16 accelerometer_mode # Accelerometer measurement mode +float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 +uint64 accelerometer_timestamp # Accelerometer timestamp +uint32 accelerometer_errcount # Error counter for accel 0 +float32 accelerometer_temp # Temperature of accel 0 + +int16[3] magnetometer_raw # Raw magnetic field in NED body frame +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss +int16 magnetometer_mode # Magnetometer measurement mode +float32 magnetometer_range_ga # measurement range in Gauss +float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor +uint64 magnetometer_timestamp # Magnetometer timestamp +uint32 magnetometer_errcount # Error counter for mag 0 +float32 magnetometer_temp # Temperature of mag 0 + +int16[3] gyro1_raw # Raw sensor values of angular velocity +float32[3] gyro1_rad_s # Angular velocity in radian per seconds +uint64 gyro1_timestamp # Gyro timestamp +uint32 gyro1_errcount # Error counter for gyro 1 +float32 gyro1_temp # Temperature of gyro 1 + +int16[3] accelerometer1_raw # Raw acceleration in NED body frame +float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer1_timestamp # Accelerometer timestamp +uint32 accelerometer1_errcount # Error counter for accel 1 +float32 accelerometer1_temp # Temperature of accel 1 + +int16[3] magnetometer1_raw # Raw magnetic field in NED body frame +float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer1_timestamp # Magnetometer timestamp +uint32 magnetometer1_errcount # Error counter for mag 1 +float32 magnetometer1_temp # Temperature of mag 1 + +int16[3] gyro2_raw # Raw sensor values of angular velocity +float32[3] gyro2_rad_s # Angular velocity in radian per seconds +uint64 gyro2_timestamp # Gyro timestamp +uint32 gyro2_errcount # Error counter for gyro 1 +float32 gyro2_temp # Temperature of gyro 1 + +int16[3] accelerometer2_raw # Raw acceleration in NED body frame +float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer2_timestamp # Accelerometer timestamp +uint32 accelerometer2_errcount # Error counter for accel 2 +float32 accelerometer2_temp # Temperature of accel 2 + +int16[3] magnetometer2_raw # Raw magnetic field in NED body frame +float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer2_timestamp # Magnetometer timestamp +uint32 magnetometer2_errcount # Error counter for mag 2 +float32 magnetometer2_temp # Temperature of mag 2 + +float32 baro_pres_mbar # Barometric pressure, already temp. comp. +float32 baro_alt_meter # Altitude, already temp. comp. +float32 baro_temp_celcius # Temperature in degrees celsius +uint64 baro_timestamp # Barometer timestamp + +float32 baro1_pres_mbar # Barometric pressure, already temp. comp. +float32 baro1_alt_meter # Altitude, already temp. comp. +float32 baro1_temp_celcius # Temperature in degrees celsius +uint64 baro1_timestamp # Barometer timestamp + +float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 +uint16[10] adc_mapping # Channel indices of each of these values +float32 mcu_temp_celcius # Internal temperature measurement of MCU + +float32 differential_pressure_pa # Airspeed sensor differential pressure +uint64 differential_pressure_timestamp # Last measurement timestamp +float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading + +float32 differential_pressure1_pa # Airspeed sensor differential pressure +uint64 differential_pressure1_timestamp # Last measurement timestamp +float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 0c02da247f..9e75534236 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -114,11 +114,14 @@ int Simulator::start(int argc, char *argv[]) PX4_INFO("Simulator started"); drv_led_start(); if (argv[2][1] == 's') { + _instance->initializeSensorData(); #ifndef __PX4_QURT - _instance->updateSamples(); + // Update sensor data + _instance->pollForMAVLinkMessages(false); #endif } else { - _instance->publishSensorsCombined(); + // Update sensor data + _instance->pollForMAVLinkMessages(true); } } else { @@ -128,71 +131,6 @@ int Simulator::start(int argc, char *argv[]) return ret; } -void Simulator::publishSensorsCombined() { - - struct baro_report baro; - memset(&baro,0,sizeof(baro)); - baro.pressure = 120000.0f; - - // acceleration report - struct accel_report accel; - memset(&accel,0,sizeof(accel)); - accel.z = 9.81f; - accel.range_m_s2 = 80.0f; - - // gyro report - struct gyro_report gyro; - memset(&gyro, 0 ,sizeof(gyro)); - - // mag report - struct mag_report mag; - memset(&mag, 0 ,sizeof(mag)); - // init publishers - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - // fill sensors with some data - sensors.accelerometer_m_s2[2] = 9.81f; - sensors.magnetometer_ga[0] = 0.2f; - sensors.timestamp = hrt_absolute_time(); - sensors.accelerometer_timestamp = hrt_absolute_time(); - sensors.magnetometer_timestamp = hrt_absolute_time(); - sensors.baro_timestamp = hrt_absolute_time(); - // advertise - _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &sensors); - - hrt_abstime time_last = hrt_absolute_time(); - uint64_t delta; - for(;;) { - delta = hrt_absolute_time() - time_last; - if(delta > (uint64_t)1000000) { - time_last = hrt_absolute_time(); - sensors.timestamp = time_last; - sensors.accelerometer_timestamp = time_last; - sensors.magnetometer_timestamp = time_last; - sensors.baro_timestamp = time_last; - baro.timestamp = time_last; - accel.timestamp = time_last; - gyro.timestamp = time_last; - mag.timestamp = time_last; - // publish the sensor values - //PX4_DEBUG("Publishing SensorsCombined\n"); - orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &sensors); - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } - else { - usleep(1000000-delta); - } - } -} - static void usage() { PX4_WARN("Usage: simulator {start -[sc] |stop}"); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index b2ebc880cd..5d9eaa1f4b 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -39,7 +39,7 @@ #pragma once #include -#include +#include #include #include #include @@ -52,11 +52,6 @@ #include #include #include -#ifndef __PX4_QURT -#include -#include -#endif - namespace simulator { // FIXME - what is the endianness of these on actual device? @@ -118,11 +113,11 @@ struct RawGPSData { template class Report { public: Report(int readers) : - _readidx(0), - _max_readers(readers), - _report_len(sizeof(RType)) + _readidx(0), + _max_readers(readers), + _report_len(sizeof(RType)) { - sem_init(&_lock, 0, _max_readers); + sem_init(&_lock, 0, _max_readers); } ~Report() {}; @@ -148,11 +143,11 @@ public: protected: void read_lock() { sem_wait(&_lock); } void read_unlock() { sem_post(&_lock); } - void write_lock() + void write_lock() { for (int i=0; i<_max_readers; i++) { - sem_wait(&_lock); - } + sem_wait(&_lock); + } } void write_unlock() { @@ -209,14 +204,15 @@ private: _baro(1), _mag(1), _gps(1), - _sensor_combined_pub(nullptr) #ifndef __PX4_QURT - , + _accel_pub(nullptr), + _baro_pub(nullptr), + _gyro_pub(nullptr), + _mag_pub(nullptr), _rc_channels_pub(nullptr), _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), _manual_sub(-1), - _sensor{}, _rc_input{}, _actuators{}, _attitude{}, @@ -225,17 +221,15 @@ private: {} ~Simulator() { _instance=NULL; } -#ifndef __PX4_QURT - void updateSamples(); -#endif + void initializeSensorData(); static Simulator *_instance; // simulated sensor instances - simulator::Report _accel; + simulator::Report _accel; simulator::Report _mpu; simulator::Report _baro; - simulator::Report _mag; + simulator::Report _mag; simulator::Report _gps; // uORB publisher handlers @@ -243,10 +237,9 @@ private: orb_advert_t _baro_pub; orb_advert_t _gyro_pub; orb_advert_t _mag_pub; - orb_advert_t _sensor_combined_pub; // class methods - void publishSensorsCombined(); + int publish_sensor_topics(mavlink_hil_sensor_t *imu); #ifndef __PX4_QURT // uORB publisher handlers @@ -258,23 +251,19 @@ private: int _manual_sub; // uORB data containers - struct sensor_combined_s _sensor; struct rc_input_values _rc_input; struct actuator_outputs_s _actuators; struct vehicle_attitude_s _attitude; struct manual_control_setpoint_s _manual; - int _fd; - unsigned char _buf[200]; - struct sockaddr_in _srcaddr; - socklen_t _addrlen = sizeof(_srcaddr); - void poll_actuators(); - void handle_message(mavlink_message_t *msg); + void handle_message(mavlink_message_t *msg, bool publish); void send_controls(); + void pollForMAVLinkMessages(bool publish); + void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); - void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu); + void update_sensors(mavlink_hil_sensor_t *imu); void update_gps(mavlink_hil_gps_t *gps_sim); static void *sending_trampoline(void *); void send(); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 0c411edca5..cc4b871f02 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -35,9 +35,10 @@ #include #include "simulator.h" #include "errno.h" +#include #include - -using namespace simulator; +#include +#include #define SEND_INTERVAL 20 #define UDP_PORT 14560 @@ -49,9 +50,17 @@ using namespace simulator; static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; static int openUart(const char *uart_name, int baud); +static int _fd; +static unsigned char _buf[200]; +sockaddr_in _srcaddr; +static socklen_t _addrlen = sizeof(_srcaddr); + +using namespace simulator; + void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { float out[8]; @@ -93,6 +102,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { void Simulator::send_controls() { mavlink_hil_controls_t msg; pack_actuator_message(msg); + //PX4_WARN("Sending HIL_CONTROLS msg"); send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); } @@ -102,6 +112,17 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t rc->channel_count = rc_channels->chancount; rc->rssi = rc_channels->rssi; +/* PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d", + rc_channels->chan1_raw, + rc_channels->chan2_raw, + rc_channels->chan3_raw, + rc_channels->chan4_raw, + rc_channels->chan5_raw, + rc_channels->chan6_raw, + rc_channels->chan7_raw, + rc_channels->chan8_raw); +*/ + rc->values[0] = rc_channels->chan1_raw; rc->values[1] = rc_channels->chan2_raw; rc->values[2] = rc_channels->chan3_raw; @@ -122,7 +143,7 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t rc->values[17] = rc_channels->chan18_raw; } -void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) { +void Simulator::update_sensors(mavlink_hil_sensor_t *imu) { // write sensor data to memory so that drivers can copy data from there RawMPUData mpu; mpu.accel_x = imu->xacc; @@ -174,36 +195,42 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) { gps.satellites_visible = gps_sim->satellites_visible; write_gps_data((void *)&gps); - } -void Simulator::handle_message(mavlink_message_t *msg) { +void Simulator::handle_message(mavlink_message_t *msg, bool publish) { switch(msg->msgid) { - case MAVLINK_MSG_ID_HIL_SENSOR: - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); - update_sensors(&_sensor, &imu); - break; + case MAVLINK_MSG_ID_HIL_SENSOR: + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); + if (publish) { + publish_sensor_topics(&imu); + } + update_sensors(&imu); + break; - case MAVLINK_MSG_ID_HIL_GPS: - mavlink_hil_gps_t gps_sim; - mavlink_msg_hil_gps_decode(msg, &gps_sim); - update_gps(&gps_sim); - break; + case MAVLINK_MSG_ID_HIL_GPS: + mavlink_hil_gps_t gps_sim; + mavlink_msg_hil_gps_decode(msg, &gps_sim); + if (publish) { + //PX4_WARN("FIXME: Need to publish GPS topic. Not done yet."); + } + update_gps(&gps_sim); + break; - case MAVLINK_MSG_ID_RC_CHANNELS: + case MAVLINK_MSG_ID_RC_CHANNELS: + mavlink_rc_channels_t rc_channels; + mavlink_msg_rc_channels_decode(msg, &rc_channels); + fill_rc_input_msg(&_rc_input, &rc_channels); - mavlink_rc_channels_t rc_channels; - mavlink_msg_rc_channels_decode(msg, &rc_channels); - fill_rc_input_msg(&_rc_input, &rc_channels); - - // publish message + // publish message + if (publish) { if(_rc_channels_pub == nullptr) { _rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input); } else { orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input); } - break; + } + break; } } @@ -246,6 +273,7 @@ void Simulator::poll_actuators() { bool updated; orb_check(_actuator_outputs_sub, &updated); if(updated) { + //PX4_WARN("Received actuator_output0 orb_topic"); orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); } } @@ -286,12 +314,8 @@ void Simulator::send() { } } -void Simulator::updateSamples() +void Simulator::initializeSensorData() { - // udp socket data - struct sockaddr_in _myaddr; - const int _port = UDP_PORT; - struct baro_report baro; memset(&baro,0,sizeof(baro)); baro.pressure = 120000.0f; @@ -309,6 +333,13 @@ void Simulator::updateSamples() // mag report struct mag_report mag; memset(&mag, 0 ,sizeof(mag)); +} + +void Simulator::pollForMAVLinkMessages(bool publish) +{ + // udp socket data + struct sockaddr_in _myaddr; + const int _port = UDP_PORT; // try to setup udp socket for communcation with simulator memset((char *)&_myaddr, 0, sizeof(_myaddr)); @@ -372,9 +403,11 @@ void Simulator::updateSamples() while (pret <= 0) { pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100); } + PX4_WARN("Found initial message, pret = %d",pret); if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + PX4_WARN("Sending initial controls message to jMAVSim."); send_controls(); } @@ -413,7 +446,7 @@ void Simulator::updateSamples() if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) { // have a message, handle it - handle_message(&msg); + handle_message(&msg, publish); } } } @@ -430,7 +463,7 @@ void Simulator::updateSamples() if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) { // have a message, handle it - handle_message(&msg); + handle_message(&msg, publish); } } } @@ -528,8 +561,8 @@ int openUart(const char *uart_name, int baud) } - // Make raw - cfmakeraw(&uart_config); + // Make raw + cfmakeraw(&uart_config); if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERR SET CONF %s\n", uart_name); @@ -537,5 +570,113 @@ int openUart(const char *uart_name, int baud) return -1; } - return uart_fd; + return uart_fd; +} + +int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) { + + + //uint64_t timestamp = imu->time_usec; + uint64_t timestamp = hrt_absolute_time(); + + if((imu->fields_updated & 0x1FFF)!=0x1FFF) { + PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated. Got %08x",imu->fields_updated); + } + /* + static int count=0; + static uint64_t last_timestamp=0; + count++; + if (!(count % 200)) { + PX4_WARN("TIME : %lu, dt: %lu", + (unsigned long) timestamp,(unsigned long) timestamp - (unsigned long) last_timestamp); + PX4_WARN("IMU : %f %f %f",imu->xgyro,imu->ygyro,imu->zgyro); + PX4_WARN("ACCEL: %f %f %f",imu->xacc,imu->yacc,imu->zacc); + PX4_WARN("MAG : %f %f %f",imu->xmag,imu->ymag,imu->zmag); + PX4_WARN("BARO : %f %f %f",imu->abs_pressure,imu->pressure_alt,imu->temperature); + } + last_timestamp = timestamp; + */ + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); + + gyro.timestamp = timestamp; + gyro.x_raw = imu->xgyro * 1000.0f; + gyro.y_raw = imu->ygyro * 1000.0f; + gyro.z_raw = imu->zgyro * 1000.0f; + gyro.x = imu->xgyro; + gyro.y = imu->ygyro; + gyro.z = imu->zgyro; + + if (_gyro_pub == nullptr) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } + + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); + + accel.timestamp = timestamp; + accel.x_raw = imu->xacc / mg2ms2; + accel.y_raw = imu->yacc / mg2ms2; + accel.z_raw = imu->zacc / mg2ms2; + accel.x = imu->xacc; + accel.y = imu->yacc; + accel.z = imu->zacc; + + if (_accel_pub == nullptr) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } + + /* magnetometer */ + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); + + mag.timestamp = timestamp; + mag.x_raw = imu->xmag * 1000.0f; + mag.y_raw = imu->ymag * 1000.0f; + mag.z_raw = imu->zmag * 1000.0f; + mag.x = imu->xmag; + mag.y = imu->ymag; + mag.z = imu->zmag; + + if (_mag_pub == nullptr) { + /* publish to the first mag topic */ + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } + + /* baro */ + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); + + baro.timestamp = timestamp; + baro.pressure = imu->abs_pressure; + baro.altitude = imu->pressure_alt; + baro.temperature = imu->temperature; + + if (_baro_pub == nullptr) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } + } + + return OK; } From 1efabba6a624b4da394b985e3fb8898997bede7e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 18:13:59 -0700 Subject: [PATCH 242/493] SITL: Added HIL message used by simulator The simulator uses this messgage to get incoming data from jMAVSim that it publishes as sensor data that is consumed by the sensors module. Signed-off-by: Mark Charlebois --- src/modules/uORB/objects_common.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index de628b3f86..cc83b932cf 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -72,6 +72,9 @@ ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); #include "topics/sensor_combined.h" ORB_DEFINE(sensor_combined, struct sensor_combined_s); +#include "topics/hil_sensor.h" +ORB_DEFINE(hil_sensor, struct hil_sensor_s); + #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); From f6af5dc3123b65bb49987d8c1bb0b7dfa1bea25d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 20:20:45 -0700 Subject: [PATCH 243/493] Added hil_sensor to Subscription.cpp Signed-off-by: Mark Charlebois --- src/modules/uORB/Subscription.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 0c9433f036..3554b497d7 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -42,6 +42,7 @@ #include "topics/vehicle_gps_position.h" #include "topics/satellite_info.h" #include "topics/sensor_combined.h" +#include "topics/hil_sensor.h" #include "topics/vehicle_attitude.h" #include "topics/vehicle_global_position.h" #include "topics/encoders.h" @@ -63,21 +64,24 @@ template Subscription::Subscription( const struct orb_metadata *meta, unsigned interval, - List * list) : + List *list) : T(), // initialize data structure to zero - SubscriptionNode(meta, interval, list) { + SubscriptionNode(meta, interval, list) +{ } template Subscription::~Subscription() {} template -void * Subscription::getDataVoidPtr() { +void *Subscription::getDataVoidPtr() +{ return (void *)(T *)(this); } template -T Subscription::getData() { +T Subscription::getData() +{ return T(*this); } @@ -86,6 +90,7 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; From 31e4b4e17bb80006e8928fd9dde8ded078327ebc Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 21:09:00 -0700 Subject: [PATCH 244/493] SITL: fixed formatting of config_posix_sitl.mk Signed-off-by: Mark Charlebois --- makefiles/posix/config_posix_sitl.mk | 33 ++++++++++++++-------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index c1eba88b3d..14678aa81a 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -11,20 +11,20 @@ MODULES += drivers/hil MODULES += drivers/rgbled MODULES += drivers/led MODULES += modules/sensors -#MODULES += drivers/ms5611 +#MODULES += drivers/ms5611 # # System commands # -MODULES += systemcmds/param -MODULES += systemcmds/mixer -#MODULES += systemcmds/esc_calib -MODULES += systemcmds/tests -#MODULES += systemcmds/reboot -MODULES += systemcmds/topic_listener -MODULES += systemcmds/ver -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot +MODULES += systemcmds/param +MODULES += systemcmds/mixer +#MODULES += systemcmds/esc_calib +MODULES += systemcmds/tests +#MODULES += systemcmds/reboot +MODULES += systemcmds/topic_listener +MODULES += systemcmds/ver +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot # # General system control @@ -35,6 +35,7 @@ MODULES += modules/mavlink # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_q MODULES += modules/ekf_att_pos_estimator MODULES += modules/attitude_estimator_q MODULES += modules/position_estimator_inav @@ -45,7 +46,7 @@ MODULES += modules/position_estimator_inav MODULES += modules/navigator MODULES += modules/mc_pos_control MODULES += modules/mc_att_control -MODULES += modules/land_detector +MODULES += modules/land_detector # # Library modules @@ -83,12 +84,12 @@ MODULES += platforms/posix/drivers/gpssim # # Unit tests # -#MODULES += platforms/posix/tests/hello -#MODULES += platforms/posix/tests/vcdev_test -#MODULES += platforms/posix/tests/hrt_test -#MODULES += platforms/posix/tests/wqueue +#MODULES += platforms/posix/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue # # muorb fastrpc changes. # -#MODULES += $(PX4_BASE)../muorb_krait +#MODULES += $(PX4_BASE)../muorb_krait From 0c72d66ece22b219ff5bfa42554752b5715b6b9a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 21:26:00 -0700 Subject: [PATCH 245/493] uORBManager: allocate instance on first use Previously _Instance was statically initialized. Now it is allocated at first use. Signed-off-by: Mark Charlebois --- src/modules/uORB/module.mk | 11 +++++++---- src/modules/uORB/uORBManager.hpp | 2 +- src/modules/uORB/uORBManager_nuttx.cpp | 8 ++++++-- src/modules/uORB/uORBManager_posix.cpp | 8 ++++++-- 4 files changed, 20 insertions(+), 9 deletions(-) diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 30a997b317..6a71fc25f4 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -50,15 +50,18 @@ SRCS = uORBDevices_posix.cpp \ endif ifeq ($(PX4_TARGET_OS),posix) -SRCS += uORBTest_UnitTest.cpp +SRCS += uORBTest_UnitTest.cpp endif ifeq ($(PX4_TARGET_OS),posix-arm) -SRCS += uORBTest_UnitTest.cpp +SRCS += uORBTest_UnitTest.cpp +endif + +ifneq ($(PX4_TARGET_OS),qurt) +SRCS += Publication.cpp \ + Subscription.cpp endif SRCS += objects_common.cpp \ - Publication.cpp \ - Subscription.cpp \ uORBUtils.cpp \ uORB.cpp \ uORBMain.cpp diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index ebe673ba2d..f998c8252c 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -346,7 +346,7 @@ private: // class methods ); private: // data members - static Manager _Instance; + static Manager *_Instance; // the communicator channel instance. uORBCommunicator::IChannel *_comm_channel; ORBSet _remote_subscriber_topics; diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp index cd6020b09c..8b2051aacf 100644 --- a/src/modules/uORB/uORBManager_nuttx.cpp +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -42,13 +42,17 @@ //========================= Static initializations ================= -uORB::Manager uORB::Manager::_Instance; +uORB::Manager *uORB::Manager::_Instance = nullptr; //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- uORB::Manager *uORB::Manager::get_instance() { - return &_Instance; + if (_Instance == nullptr) { + _Instance = new uORB::Manager(); + } + + return _Instance; } //----------------------------------------------------------------------------- diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index f8b876e629..68bfa3f7fe 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -44,13 +44,17 @@ //========================= Static initializations ================= -uORB::Manager uORB::Manager::_Instance; +uORB::Manager *uORB::Manager::_Instance = nullptr; //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- uORB::Manager *uORB::Manager::get_instance() { - return &_Instance; + if (_Instance == nullptr) { + _Instance = new uORB::Manager(); + } + + return _Instance; } //----------------------------------------------------------------------------- From d219076d52dd11d6b790c682c58d4311b478f889 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 21:33:21 -0700 Subject: [PATCH 246/493] POSIX: added muorb tests Unit tests for muorb on posix build. These run on the Krait processor. Signed-off-by: Mark Charlebois --- src/platforms/posix/tests/muorb/module.mk | 6 +- .../posix/tests/muorb/muorb_test_example.cpp | 79 +++++++++++++++++-- .../posix/tests/muorb/muorb_test_example.h | 2 + .../posix/tests/muorb/muorb_test_main.cpp | 7 -- 4 files changed, 77 insertions(+), 17 deletions(-) diff --git a/src/platforms/posix/tests/muorb/module.mk b/src/platforms/posix/tests/muorb/module.mk index 15bf4824e9..880fe82007 100644 --- a/src/platforms/posix/tests/muorb/module.mk +++ b/src/platforms/posix/tests/muorb/module.mk @@ -37,9 +37,9 @@ MODULE_COMMAND = muorb_test -INCLUDE_DIRS += ${PX4_BASE}../muorb_krait \ - ${PX4_BASE}../muorb_krait/lib/include \ - ${PX4_BASE}../muorb_krait/Pal/lib +INCLUDE_DIRS += \ + $(EXT_MUORB_LIB_ROOT)/krait/include \ + $(PX4_BASE)src/modules/muorb/krait SRCS = muorb_test_main.cpp \ muorb_test_start_posix.cpp \ diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.cpp b/src/platforms/posix/tests/muorb/muorb_test_example.cpp index 37dfec5ed3..3bae353f2f 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_example.cpp +++ b/src/platforms/posix/tests/muorb/muorb_test_example.cpp @@ -48,9 +48,16 @@ px4::AppState MuorbTestExample::appState; int MuorbTestExample::main() { + int rc; appState.setRunning(true); + rc = PingPongTest(); + appState.setRunning(false); + return rc; +} - int i=0; +int MuorbTestExample::DefaultTest() +{ + int i=0; orb_advert_t pub_id = orb_advertise( ORB_ID( esc_status ), & m_esc_status ); if( pub_id == 0 ) { @@ -75,9 +82,9 @@ int MuorbTestExample::main() return -1; } - while (!appState.exitRequested() && i<100) { + while (!appState.exitRequested() && i<100) { - PX4_DEBUG("[%d] Doing work...", i ); + PX4_DEBUG("[%d] Doing work...", i ); if( orb_publish( ORB_ID( esc_status ), pub_id, &m_esc_status ) == PX4_ERROR ) { PX4_ERR( "[%d]Error publishing the esc status message for iter", i ); @@ -111,8 +118,66 @@ int MuorbTestExample::main() break; } - ++i; - } - - return 0; + ++i; + } + return 0; +} + +int MuorbTestExample::PingPongTest() +{ + int i=0; + orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc ); + if( pub_id_vc == 0 ) + { + PX4_ERR( "error publishing vehicle_command" ); + return -1; + } + if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the vechile command message", i ); + return -1; + } + int sub_esc_status = orb_subscribe( ORB_ID( esc_status ) ); + if ( sub_esc_status == PX4_ERROR ) + { + PX4_ERR( "Error subscribing to esc_status topic" ); + return -1; + } + + while (!appState.exitRequested() ) { + + PX4_INFO("[%d] Doing work...", i ); + bool updated = false; + if( orb_check( sub_esc_status, &updated ) == 0 ) + { + if( updated ) + { + PX4_INFO( "[%d]ESC status is updated... reading new value", i ); + if( orb_copy( ORB_ID( esc_status ), sub_esc_status, &m_esc_status ) != 0 ) + { + PX4_ERR( "[%d]Error calling orb copy for esc status... ", i ); + break; + } + if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the vechile command message", i ); + break; + } + } + else + { + PX4_INFO( "[%d] esc status topic is not updated", i ); + } + } + else + { + PX4_ERR( "[%d]Error checking the updated status for esc status... ", i ); + break; + } + // sleep for 1 sec. + usleep( 1000000 ); + + ++i; + } + return 0; } diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.h b/src/platforms/posix/tests/muorb/muorb_test_example.h index a3625167c7..c5d699ae7d 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_example.h +++ b/src/platforms/posix/tests/muorb/muorb_test_example.h @@ -53,6 +53,8 @@ public: static px4::AppState appState; /* track requests to terminate app */ private: + int DefaultTest(); + int PingPongTest(); struct esc_status_s m_esc_status; struct vehicle_command_s m_vc; diff --git a/src/platforms/posix/tests/muorb/muorb_test_main.cpp b/src/platforms/posix/tests/muorb/muorb_test_main.cpp index 6ebc0ae92c..effa9ff88b 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_main.cpp +++ b/src/platforms/posix/tests/muorb/muorb_test_main.cpp @@ -51,16 +51,9 @@ int PX4_MAIN(int argc, char **argv) PX4_DEBUG("muorb_test"); - // register the fast rpc channel with UORB. - uORB::Manager::get_instance()->set_uorb_communicator( uORB::KraitFastRpcChannel::GetInstance() ); - - // start the KaitFastRPC channel thread. - uORB::KraitFastRpcChannel::GetInstance()->Start(); - MuorbTestExample hello; hello.main(); - uORB::KraitFastRpcChannel::GetInstance()->Stop(); PX4_DEBUG("goodbye"); return 0; } From 2ea82548a4a433f36746dfb62af0c930ab86e82b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 2 Jul 2015 01:06:26 -0700 Subject: [PATCH 247/493] Change fabsf() to abs for int arg Clang complains that fabsf() is being used for an int arg. Use abs() instead. Signed-off-by: Mark Charlebois --- src/systemcmds/tests/test_rc.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index b4f68c35fb..1f150816d4 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -106,7 +106,7 @@ int test_rc(int argc, char *argv[]) /* go and check values */ for (unsigned i = 0; i < rc_input.channel_count; i++) { - if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) { + if (abs(rc_input.values[i] - rc_last.values[i]) > 20) { PX4_ERR("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]); (void)close(_rc_sub); return ERROR; From 20de4aaaa54f79d111dd5b206c7386bbe8eeb68e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 10:47:33 +0200 Subject: [PATCH 248/493] HIL driver: Output zero like the other actuator drivers do when not armed --- src/drivers/hil/hil.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index e6131b7175..c3d99e80fe 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -454,6 +454,7 @@ HIL::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + _armed = aa.armed && !aa.lockdown; } } @@ -477,7 +478,12 @@ HIL::control_callback(uintptr_t handle, { const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = controls->control[control_index]; + if (_armed) { + input = controls->control[control_index]; + } else { + /* clamp actuator to zero if not armed */ + input = 0.0f; + } return 0; } From 1cb572f48437db6607dcc60bb98e8774f54bc56e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jul 2015 18:27:01 -0700 Subject: [PATCH 249/493] POSIX: Fix MAVLink sequencing --- posix-configs/SITL/init/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index d9f94c5ce0..27795eca5c 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -7,7 +7,6 @@ param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 param set COM_RC_IN_MODE 2 dataman start -mavlink start -u 14556 -r 60000 simulator start -s param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1376256 @@ -42,6 +41,7 @@ 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 60000 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 50 -s ATTITUDE -u 14556 From ce439345c5a959cb40e23a9770893ccb61f38bff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 11:17:30 +0200 Subject: [PATCH 250/493] HIL driver: Fix build breakage --- src/drivers/hil/hil.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index c3d99e80fe..b6586856cc 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -123,7 +123,7 @@ private: bool _primary_pwm_device; volatile bool _task_should_exit; - bool _armed; + static bool _armed; MixerGroup *_mixers; @@ -163,6 +163,8 @@ HIL *g_hil; } // namespace +bool HIL::_armed = false; + HIL::HIL() : #ifdef __PX4_NUTTX CDev @@ -180,7 +182,6 @@ HIL::HIL() : _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), - _armed(false), _mixers(nullptr) { _debug_enabled = true; From b0a0e60c5f4959dac1a1517edae17eee2fed3164 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jul 2015 19:54:17 -0700 Subject: [PATCH 251/493] POSIX: Workaround for broken px4_read interface to accel --- src/modules/commander/PreflightCheck.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 1a9d0ad57b..bbf5f8ec6b 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -154,6 +154,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, goto out; } +#ifdef __PX4_NUTTX if (dynamic) { /* check measurement result range */ struct accel_report acc; @@ -176,6 +177,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, goto out; } } +#endif out: px4_close(fd); From adfd1b2579ad786686180505dcb232cf6e3dd1ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jul 2015 23:55:20 -0700 Subject: [PATCH 252/493] sensors: Ensure data is good before publishing --- src/modules/sensors/sensors.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9ce4b1f570..92c9eb511c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2187,6 +2187,8 @@ Sensors::task_main() _task_should_exit = false; + raw.timestamp = 0; + while (!_task_should_exit) { /* wait for up to 50ms for data */ @@ -2229,7 +2231,7 @@ Sensors::task_main() diff_pres_poll(raw); /* Inform other processes that new data is available to copy */ - if (_publishing) { + if (_publishing && raw.timestamp > 0) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); } From efb7d9393e7e67a1d175372944c5476ed08a4def Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jul 2015 23:59:39 -0700 Subject: [PATCH 253/493] POSIX: Set SITL gains back to normal vehicle defaults --- posix-configs/SITL/init/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 27795eca5c..9cbd750c80 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -1,8 +1,8 @@ uorb start param load param set MAV_TYPE 2 -param set MC_PITCHRATE_P 0.05 -param set MC_ROLLRATE_P 0.05 +param set MC_PITCHRATE_P 0.15 +param set MC_ROLLRATE_P 0.15 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 param set COM_RC_IN_MODE 2 From e19a068ebb8f0d3d1b21bceff36a72beb6180c21 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 00:04:06 -0700 Subject: [PATCH 254/493] Better SITL gains for yaw --- posix-configs/SITL/init/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 9cbd750c80..bf4bd501df 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -3,6 +3,8 @@ param load param set MAV_TYPE 2 param set MC_PITCHRATE_P 0.15 param set MC_ROLLRATE_P 0.15 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 param set COM_RC_IN_MODE 2 From 9c60154a28492a4a351ac552d4d7e8a43db082e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 17:27:28 +0200 Subject: [PATCH 255/493] POSIX HRT Driver: Count from 0, not UNIX epoch --- src/platforms/posix/px4_layer/drv_hrt.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 0be34240c6..0a45644481 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -61,6 +61,7 @@ static void hrt_call_reschedule(void); static sem_t _hrt_lock; static struct work_s _hrt_work; +static hrt_abstime px4_timestart = 0; static void hrt_call_invoke(void); @@ -86,7 +87,6 @@ static void hrt_unlock(void) #define clockid_t int static double px4_timebase = 0.0; -static uint64_t px4_timestart = 0; int clock_gettime(clockid_t clk_id, struct timespec *t) { @@ -119,8 +119,13 @@ hrt_abstime hrt_absolute_time(void) { struct timespec ts; + if (!px4_timestart) { + clock_gettime(CLOCK_MONOTONIC, &ts); + px4_timestart = ts_to_abstime(&ts); + } + clock_gettime(CLOCK_MONOTONIC, &ts); - return ts_to_abstime(&ts); + return ts_to_abstime(&ts) - px4_timestart; } /* From 10eb5de5ce4ed4c3227efd90c6fa6e5b4813fa6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 22:23:23 +0200 Subject: [PATCH 256/493] Add vehicle config list for downstream config tools --- Tools/dist/vehicle_configs.xml | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 Tools/dist/vehicle_configs.xml diff --git a/Tools/dist/vehicle_configs.xml b/Tools/dist/vehicle_configs.xml new file mode 100644 index 0000000000..9af2469cf2 --- /dev/null +++ b/Tools/dist/vehicle_configs.xml @@ -0,0 +1,23 @@ + + + + Standard 8" Prop Quadrotor (x) + Standard quadrotor configuration in x configuration for 8-" propellers + ROMFS/px4fmu_common/mixers/quad_x.main.mix + + + Standard 8" Prop Quadrotor (+) + Standard quadrotor configuration in + configuration for 8-" propellers + ROMFS/px4fmu_common/mixers/quad_+.main.mix + + + Standard 8" Prop Quadrotor (x) + Standard quadrotor configuration in x configuration for 8-" propellers + ROMFS/px4fmu_common/mixers/quad_x.main.mix + + + Zeta Science Wing Wing Z-84 + Configuration for a small flying wing. + ROMFS/px4fmu_common/mixers/wingwing.main.mix + + From 39fd3c1d4f221e4e3ed599ad46c9db34feb9aa63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 22:29:19 +0200 Subject: [PATCH 257/493] Update vehicle config mixer URLs --- Tools/dist/vehicle_configs.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Tools/dist/vehicle_configs.xml b/Tools/dist/vehicle_configs.xml index 9af2469cf2..76d7ba11eb 100644 --- a/Tools/dist/vehicle_configs.xml +++ b/Tools/dist/vehicle_configs.xml @@ -3,21 +3,21 @@ Standard 8" Prop Quadrotor (x) Standard quadrotor configuration in x configuration for 8-" propellers - ROMFS/px4fmu_common/mixers/quad_x.main.mix + /etc/mixers/quad_x.main.mix Standard 8" Prop Quadrotor (+) Standard quadrotor configuration in + configuration for 8-" propellers - ROMFS/px4fmu_common/mixers/quad_+.main.mix + /etc/mixers/quad_+.main.mix Standard 8" Prop Quadrotor (x) Standard quadrotor configuration in x configuration for 8-" propellers - ROMFS/px4fmu_common/mixers/quad_x.main.mix + /etc/mixers/quad_x.main.mix Zeta Science Wing Wing Z-84 Configuration for a small flying wing. - ROMFS/px4fmu_common/mixers/wingwing.main.mix + /etc/mixers/wingwing.main.mix From 51c515a14faa796c84e854e62acfe23941be73d7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 2 Jul 2015 14:46:37 -0700 Subject: [PATCH 258/493] Generic AETR and AERT airframes Bixler converted to generic AERT --- .../init.d/{2101_hk_bixler => 2101_fw_AERT} | 0 ROMFS/px4fmu_common/init.d/2104_fw_AETR | 5 ++ ROMFS/px4fmu_common/init.d/rc.autostart | 8 +- ROMFS/px4fmu_common/mixers/AETR.main.mix | 84 +++++++++++++++++++ 4 files changed, 96 insertions(+), 1 deletion(-) rename ROMFS/px4fmu_common/init.d/{2101_hk_bixler => 2101_fw_AERT} (100%) create mode 100644 ROMFS/px4fmu_common/init.d/2104_fw_AETR create mode 100644 ROMFS/px4fmu_common/mixers/AETR.main.mix diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_fw_AERT similarity index 100% rename from ROMFS/px4fmu_common/init.d/2101_hk_bixler rename to ROMFS/px4fmu_common/init.d/2101_fw_AERT diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR new file mode 100644 index 0000000000..bb4390b1d3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -0,0 +1,5 @@ +#!nsh + +sh /etc/init.d/rc.fw_defaults + +set MIXER AETR diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index a45ceeb276..ec405accac 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -65,7 +65,7 @@ fi if param compare SYS_AUTOSTART 2101 101 then - sh /etc/init.d/2101_hk_bixler + sh /etc/init.d/2101_fw_AERT set MODE custom fi @@ -81,6 +81,12 @@ then set MODE custom fi +if param compare SYS_AUTOSTART 2104 +then + sh /etc/init.d/2104_fw_AETR + set MODE custom +fi + # # Flying wing # diff --git a/ROMFS/px4fmu_common/mixers/AETR.main.mix b/ROMFS/px4fmu_common/mixers/AETR.main.mix new file mode 100644 index 0000000000..8bd3613128 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AETR.main.mix @@ -0,0 +1,84 @@ +Aileron/Elevator/Throttle/Rudder mixer for PX4FMU +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator and throttle controls using PX4FMU. The configuration +assumes the aileron servo(s) are connected to PX4FMU servo output 0, the +elevator to output 1, the throttle to output 2 and the rudder to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels, +using the payload control group +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 3 10000 10000 0 -10000 10000 From e1c050df096721a1e49cdebaa9df8118bbaff0ca Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 2 Jul 2015 14:46:37 -0700 Subject: [PATCH 259/493] Generic AETR and AERT airframes Bixler converted to generic AERT --- .../init.d/{2101_hk_bixler => 2101_fw_AERT} | 0 ROMFS/px4fmu_common/init.d/2104_fw_AETR | 5 ++ ROMFS/px4fmu_common/init.d/rc.autostart | 8 +- ROMFS/px4fmu_common/mixers/AETR.main.mix | 84 +++++++++++++++++++ 4 files changed, 96 insertions(+), 1 deletion(-) rename ROMFS/px4fmu_common/init.d/{2101_hk_bixler => 2101_fw_AERT} (100%) create mode 100644 ROMFS/px4fmu_common/init.d/2104_fw_AETR create mode 100644 ROMFS/px4fmu_common/mixers/AETR.main.mix diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_fw_AERT similarity index 100% rename from ROMFS/px4fmu_common/init.d/2101_hk_bixler rename to ROMFS/px4fmu_common/init.d/2101_fw_AERT diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR new file mode 100644 index 0000000000..bb4390b1d3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -0,0 +1,5 @@ +#!nsh + +sh /etc/init.d/rc.fw_defaults + +set MIXER AETR diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index a45ceeb276..ec405accac 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -65,7 +65,7 @@ fi if param compare SYS_AUTOSTART 2101 101 then - sh /etc/init.d/2101_hk_bixler + sh /etc/init.d/2101_fw_AERT set MODE custom fi @@ -81,6 +81,12 @@ then set MODE custom fi +if param compare SYS_AUTOSTART 2104 +then + sh /etc/init.d/2104_fw_AETR + set MODE custom +fi + # # Flying wing # diff --git a/ROMFS/px4fmu_common/mixers/AETR.main.mix b/ROMFS/px4fmu_common/mixers/AETR.main.mix new file mode 100644 index 0000000000..8bd3613128 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AETR.main.mix @@ -0,0 +1,84 @@ +Aileron/Elevator/Throttle/Rudder mixer for PX4FMU +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator and throttle controls using PX4FMU. The configuration +assumes the aileron servo(s) are connected to PX4FMU servo output 0, the +elevator to output 1, the throttle to output 2 and the rudder to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels, +using the payload control group +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 3 10000 10000 0 -10000 10000 From 88d200e3a471716a8b3e98a3c28131ff18191ad6 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 3 Jul 2015 14:36:55 +0200 Subject: [PATCH 260/493] set altitude control flag for velocity control --- src/modules/commander/commander.cpp | 3 ++- src/platforms/ros/nodes/commander/commander.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5ac8e9ca8b..6c0f1da8cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2637,7 +2637,8 @@ set_control_mode() control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position; - control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; + control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; break; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index abaa6fc60d..54086cfd4b 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -135,7 +135,8 @@ void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboar msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position; - msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position; + msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_position; } void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, From 9451d2285026de60e47f726dcfa7d39c886721c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 14:54:25 +0200 Subject: [PATCH 261/493] Aerocore: Retire attitude-only EKF --- makefiles/config_aerocore_default.mk | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index c906d54189..0ce01d499d 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -48,11 +48,12 @@ MODULES += modules/navigator MODULES += modules/mavlink # -# Estimation modules (EKF/ SO3 / other filters) +# Estimation modules (EKF / other filters) # -MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 +# Too high RAM usage due to static allocations +#MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator +MODULES += modules/attitude_estimator_q MODULES += modules/position_estimator_inav # From ecaa25ba1a9d6f1ac0f1df2de5b20355a6fb4b95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 14:55:53 +0200 Subject: [PATCH 262/493] FMUv2: Retire attitude only EKF --- makefiles/config_px4fmu-v2_default.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 7884b94cb0..0998ebf9f8 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -76,7 +76,8 @@ MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) # -MODULES += modules/attitude_estimator_ekf +# Too high RAM usage due to static allocations +#MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_q MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav From e23459e8500ca2ea712cc756060019176bec17a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 23:17:07 +0200 Subject: [PATCH 263/493] Commander: Fix dynamic battery scaling, proposed by @orangelynx. Fixes #2523. --- src/modules/commander/commander_helper.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 362a707c03..0d5abef9ab 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -382,8 +382,12 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized); - float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty)); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized); + /* the range from full to empty is the same for batteries under load and without load, + * since the voltage drop applies to both the full and empty state + */ + float voltage_range = (bat_v_full - bat_v_empty) + float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ From 9e223f0c268ff79436b5f4ce5673ffe1061c3cfe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 23:17:07 +0200 Subject: [PATCH 264/493] Commander: Fix dynamic battery scaling, proposed by @orangelynx. Fixes #2523. --- src/modules/commander/commander_helper.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 30a599c0ab..0774db241e 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -384,8 +384,12 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized); - float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty)); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized); + /* the range from full to empty is the same for batteries under load and without load, + * since the voltage drop applies to both the full and empty state + */ + float voltage_range = (bat_v_full - bat_v_empty) + float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ From f8f412fc61108a1b1962bb096b18092ed564c175 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 23:50:47 +0200 Subject: [PATCH 265/493] Commander: Compile fix --- src/modules/commander/commander_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0d5abef9ab..68949eec1d 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -386,7 +386,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float /* the range from full to empty is the same for batteries under load and without load, * since the voltage drop applies to both the full and empty state */ - float voltage_range = (bat_v_full - bat_v_empty) + float voltage_range = (bat_v_full - bat_v_empty); float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range); if (bat_capacity > 0.0f) { From 615affdef9cb84d8c50103bb910265cf41c619c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 23:51:45 +0200 Subject: [PATCH 266/493] S.BUS Output: deliver the disarmed PWM values --- src/modules/px4iofirmware/mixer.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b5d93daea7..0106fa1eb7 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -287,15 +287,18 @@ mixer_tick(void) } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servo_disarmed[i]); + } /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) - sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + } - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) - sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { + sbus2_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + } } } From 234aeb642bf0f06443559a0d273fecb5d41846d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 23:50:47 +0200 Subject: [PATCH 267/493] Commander: Compile fix --- src/modules/commander/commander_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0774db241e..9eac930589 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -388,7 +388,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float /* the range from full to empty is the same for batteries under load and without load, * since the voltage drop applies to both the full and empty state */ - float voltage_range = (bat_v_full - bat_v_empty) + float voltage_range = (bat_v_full - bat_v_empty); float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range); if (bat_capacity > 0.0f) { From b27b864cf0981274ec96ece16fd3969803c91ffc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 10:45:01 +0200 Subject: [PATCH 268/493] Commander: Only copy global position is valid. This is because the app assumed that it only gets published once valid. --- src/modules/commander/commander.cpp | 28 +++++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c0f1da8cb..f6fa5e6813 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1479,7 +1479,22 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { /* position changed */ - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + vehicle_global_position_s gpos; + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos); + + /* copy to global struct if valid, with hysteresis */ + + // XXX consolidate this with local position handling and timeouts after release + // but we want a low-risk change now. + if (status.condition_global_position_valid) { + if (gpos.eph < eph_threshold * 2.5f) { + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + } + } else { + if (gpos.eph < eph_threshold) { + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + } + } } /* update local position estimate */ @@ -1492,17 +1507,16 @@ int commander_thread_main(int argc, char *argv[]) //update condition_global_position_valid //Global positions are only published by the estimators if they are valid - if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { + if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { //We have had no good fix for POSITION_TIMEOUT amount of time - if(status.condition_global_position_valid) { + if (status.condition_global_position_valid) { set_tune_override(TONE_GPS_WARNING_TUNE); status_changed = true; status.condition_global_position_valid = false; } - } - else if(global_position.timestamp != 0) { - //Got good global position estimate - if(!status.condition_global_position_valid) { + } else if (global_position.timestamp != 0) { + // Got good global position estimate + if (!status.condition_global_position_valid) { status_changed = true; status.condition_global_position_valid = true; } From 8f4b9c02f0f68ddc69b11c5045dac672ccb886b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 10:45:30 +0200 Subject: [PATCH 269/493] EKF: Fix for the GPS timeout logic --- .../ekf_att_pos_estimator_main.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 877bff6585..60be85b2cd 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -74,6 +74,8 @@ static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we si static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) +static constexpr float EPH_LARGE_VALUE = 1000.0f; +static constexpr float EPV_LARGE_VALUE = 1000.0f; /** * estimator app start / stop handling function @@ -924,8 +926,16 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); _global_pos.yaw = _local_pos.yaw; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + + const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; + + if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + _global_pos.eph = EPH_LARGE_VALUE; + _global_pos.epv = EPV_LARGE_VALUE; + } else { + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + } if (!isfinite(_global_pos.lat) || !isfinite(_global_pos.lon) || @@ -1424,7 +1434,7 @@ void AttitudePositionEstimatorEKF::pollData() // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, // then something is very wrong with the GPS (possibly a hardware failure or comlink error) - const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; if (dtLastGoodGPS >= POS_RESET_THRESHOLD) { From ec85918e40085f3e6133712317fa7a89fad2904f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 11:39:12 +0200 Subject: [PATCH 270/493] Set better defaults for fixed wing attitude controllers --- src/modules/fw_att_control/fw_att_control_params.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index d9ccd8bac8..234ff0c3fb 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -64,7 +64,7 @@ * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); +PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.4f); /** * Pitch rate proportional gain. @@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); /** * Pitch rate integrator gain. @@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); * @max 50.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f); /** * Maximum positive / up pitch rate. @@ -126,7 +126,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); +PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); /** * Roll to Pitch feedforward gain. @@ -161,7 +161,7 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); * @max 100.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_RR_I, 0.005f); /** * Roll Integrator Anti-Windup @@ -247,7 +247,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 10.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f); +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); /** * Pitch rate feed forward From 3671ce716ad820a4d4a5d0e6c498cad9a55ce945 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 11:39:31 +0200 Subject: [PATCH 271/493] Set better defaults for fixed wing position controllers --- .../fw_pos_control_l1_params.c | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index c00d822327..78d66afe1e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -58,7 +58,7 @@ * @max 100.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); +PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); /** * L1 damping @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); * @max 1.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); +PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f); /** * Throttle max slew rate @@ -123,10 +123,11 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); * The maximum roll the controller will output. * * @unit degrees - * @min 0.0 + * @min 35.0 + * @max 65.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); +PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); /** * Throttle limit max @@ -151,6 +152,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); * For aircraft with internal combustion engine this parameter should be set * for desired idle rpm. * + * @min 0.0 + * @max 1.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); @@ -161,6 +164,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * + * @min 0.0 + * @max 1.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); @@ -173,6 +178,8 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); * distance to the desired altitude. Mostly used for takeoff waypoints / modes. * Set to zero to disable climbout mode (not recommended). * + * @min 0.0 + * @max 150.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); @@ -193,6 +200,8 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. * + * @min 2.0 + * @max 10.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); From 134f3d8858b373e69aba461d410dfa69e652fff9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 11:39:54 +0200 Subject: [PATCH 272/493] Wing wing config: Remove tuning gains which are close to the defaults --- ROMFS/px4fmu_common/init.d/3033_wingwing | 5 ----- 1 file changed, 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 708c34491b..3c2312fc7d 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -23,12 +23,7 @@ then param set FW_LND_TLALT 5 param set FW_THR_LND_MAX 0 param set FW_PR_FF 0.35 - param set FW_PR_I 0.005 - param set FW_PR_IMAX 0.4 - param set FW_PR_P 0.08 param set FW_RR_FF 0.6 - param set FW_RR_I 0.005 - param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 fi From 939d475ef24cb02dee5b9d267995f85791128ba4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 11:59:10 +0200 Subject: [PATCH 273/493] Output flaps in all flight modes --- .../fw_att_control/fw_att_control_main.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ccf12a0791..9cbbe80cc3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -803,6 +803,14 @@ FixedwingAttitudeControl::task_main() //warnx("_actuators_airframe.control[1] = -1.0f;"); } + /* default flaps to center */ + float flaps_control = 0.0f; + + /* map flaps by default to manual if valid */ + if (isfinite(_manual.flaps)) { + flaps_control = _manual.flaps; + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { @@ -922,7 +930,6 @@ FixedwingAttitudeControl::task_main() /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; - _actuators.control[4] = _manual.flaps; /* * in manual mode no external source should / does emit attitude setpoints. @@ -1085,13 +1092,13 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.y + _parameters.trim_roll; - _actuators.control[1] = -_manual.x + _parameters.trim_pitch; - _actuators.control[2] = _manual.r + _parameters.trim_yaw; - _actuators.control[3] = _manual.z; - _actuators.control[4] = _manual.flaps; + _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll; + _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch; + _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } + _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; _actuators.control[5] = _manual.aux1; _actuators.control[6] = _manual.aux2; _actuators.control[7] = _manual.aux3; From 00c87c041ad81c69e9e6df828db3e58684285a6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 17:24:55 +0200 Subject: [PATCH 274/493] EKF: Fix entirely unnecessary C++11 dependency --- .../estimator_utilities.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 284a099023..527420ba0b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -38,7 +38,6 @@ */ #include "estimator_utilities.h" -#include // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely @@ -72,6 +71,9 @@ ekf_debug(const char *fmt, ...) void ekf_debug(const char *fmt, ...) { while(0){} } #endif +/* we don't want to pull in the standard lib just to swap two floats */ +void swap_var(float &d1, float &d2); + float Vector3f::length(void) const { return sqrt(x*x + y*y + z*z); @@ -108,9 +110,9 @@ void Mat3f::identity() { Mat3f Mat3f::transpose() const { Mat3f ret = *this; - std::swap(ret.x.y, ret.y.x); - std::swap(ret.x.z, ret.z.x); - std::swap(ret.y.z, ret.z.y); + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); return ret; } @@ -223,3 +225,10 @@ Vector3f operator/(const Vector3f &vec, const float scalar) vecOut.z = vec.z / scalar; return vecOut; } + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} From 8c004fa6d85d0941e6364e3696f911d446163080 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 13:33:23 -0700 Subject: [PATCH 275/493] SITL: Move simulator startup to the beginning of the startup --- posix-configs/SITL/init/rc.fixed_wing | 45 +++++++++++++++++++++++++++ posix-configs/SITL/init/rcS | 2 +- 2 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 posix-configs/SITL/init/rc.fixed_wing diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing new file mode 100644 index 0000000000..cf8d1017c0 --- /dev/null +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -0,0 +1,45 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 1 +param set SYS_AUTOSTART 3033 +param set SYS_RESTART_TYPE 2 +param set COM_RC_IN_MODE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +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 +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +hil mode_pwm +commander start +sensors start +land_detector start fixedwing +navigator start +ekf_att_pos_estimator start +fw_att_control start +fw_pos_control_l1 start +mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 60000 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 50 -s ATTITUDE -u 14556 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index bf4bd501df..2af14522d2 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -1,4 +1,5 @@ uorb start +simulator start -s param load param set MAV_TYPE 2 param set MC_PITCHRATE_P 0.15 @@ -9,7 +10,6 @@ param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 param set COM_RC_IN_MODE 2 dataman start -simulator start -s param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1376256 param set CAL_ACC1_ID 1310720 From 52687cb8e1f3378ba599df049d7355e6b275efa1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 16:52:20 -0700 Subject: [PATCH 276/493] Rename make sitlrun to make sitl_quad --- Makefile | 7 +++++-- Tools/sitl_run.sh | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index 14345b960c..88683c8941 100644 --- a/Makefile +++ b/Makefile @@ -199,8 +199,11 @@ else export PX4_TARGET_OS=$@ endif -sitlrun: - Tools/sitl_run.sh +sitl_quad: + $(Q) Tools/sitl_run.sh posix-configs/SITL/init/rcS + +sitl_plane: + $(Q) Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing qurtrun: make PX4_TARGET_OS=qurt sim diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 94403247d9..3c65aa4eec 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -2,4 +2,4 @@ mkdir -p Build/posix_sitl.build/rootfs/fs/microsd mkdir -p Build/posix_sitl.build/rootfs/eeprom -cd Build/posix_sitl.build && ./mainapp ../../posix-configs/SITL/init/rcS +cd Build/posix_sitl.build && ./mainapp ../../$1 From 0499ddb1dd8713c20eaf5ec88fa51166d4a6e712 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 16:52:52 -0700 Subject: [PATCH 277/493] POSIX: Add debug output to show where the app returns --- src/platforms/posix/main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 7582183111..7b2f177e93 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -69,6 +69,7 @@ static void run_cmd(const vector &appargs) { arg[i] = (char *)0; cout << "Running: " << command << "\n"; apps[command](i,(char **)arg); + cout << "Returning: " << command << "\n"; // XXX hack to prevent shell returning too fast usleep(50000); } From 32bf4dc773758800be237c6830e878690d598af1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jul 2015 16:53:20 -0700 Subject: [PATCH 278/493] simulator: Add output so user knows that the simulator / system is waiting for data --- src/modules/simulator/simulator.cpp | 1 + src/modules/simulator/simulator_mavlink.cpp | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 9e75534236..b62ef3913e 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -128,6 +128,7 @@ int Simulator::start(int argc, char *argv[]) PX4_WARN("Simulator creation failed"); ret = 1; } + return ret; } diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index cc4b871f02..6871e66a7e 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -400,6 +400,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) // wait for first data from simulator and respond with first controls // this is important for the UDP communication to work int pret = -1; + PX4_WARN("Waiting for initial data on UDP.. Please connect the simulator first."); while (pret <= 0) { pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100); } @@ -535,9 +536,9 @@ int openUart(const char *uart_name, int baud) /* Try to set baud rate */ struct termios uart_config; - memset(&uart_config, 0, sizeof(uart_config)); + memset(&uart_config, 0, sizeof(uart_config)); - int termios_state; + int termios_state; /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { From 5a1af860aba7a802e5e967b7ac89ebad4b7ee7bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 08:09:12 -0700 Subject: [PATCH 279/493] Sim: Enforce boot order is correct, sim starts first --- src/modules/simulator/simulator.cpp | 9 +++++++++ src/modules/simulator/simulator.h | 7 ++++++- src/modules/simulator/simulator_mavlink.cpp | 1 + 3 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index b62ef3913e..f0345ba036 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -164,6 +164,15 @@ int simulator_main(int argc, char *argv[]) 1500, Simulator::start, argv); + + // now wait for the command to complete + while(true) { + if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) { + break; + } else { + usleep(100000); + } + } } else { diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 5d9eaa1f4b..a0901a9ad6 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -197,6 +197,8 @@ public: void write_baro_data(void *buf); void write_gps_data(void *buf); + bool isInitialized() { return _initialized; } + private: Simulator() : _accel(1), @@ -204,11 +206,12 @@ private: _baro(1), _mag(1), _gps(1), -#ifndef __PX4_QURT _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), _mag_pub(nullptr), + _initialized(false), +#ifndef __PX4_QURT _rc_channels_pub(nullptr), _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), @@ -238,6 +241,8 @@ private: orb_advert_t _gyro_pub; orb_advert_t _mag_pub; + bool _initialized; + // class methods int publish_sensor_topics(mavlink_hil_sensor_t *imu); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 6871e66a7e..52f595cd43 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -405,6 +405,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100); } PX4_WARN("Found initial message, pret = %d",pret); + _initialized = true; if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); From 46a6082a2682900d13dafc6ae503fcfbc1a4b934 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 08:09:31 -0700 Subject: [PATCH 280/493] POSIX: remove shell delay --- src/platforms/posix/main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 7b2f177e93..bc531af44d 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -70,8 +70,6 @@ static void run_cmd(const vector &appargs) { cout << "Running: " << command << "\n"; apps[command](i,(char **)arg); cout << "Returning: " << command << "\n"; - // XXX hack to prevent shell returning too fast - usleep(50000); } else { From 4372701dab5958253325baf4dbcbdafb5ec5f147 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 17:24:55 +0200 Subject: [PATCH 281/493] EKF: Fix entirely unnecessary C++11 dependency --- .../estimator_utilities.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 284a099023..527420ba0b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -38,7 +38,6 @@ */ #include "estimator_utilities.h" -#include // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely @@ -72,6 +71,9 @@ ekf_debug(const char *fmt, ...) void ekf_debug(const char *fmt, ...) { while(0){} } #endif +/* we don't want to pull in the standard lib just to swap two floats */ +void swap_var(float &d1, float &d2); + float Vector3f::length(void) const { return sqrt(x*x + y*y + z*z); @@ -108,9 +110,9 @@ void Mat3f::identity() { Mat3f Mat3f::transpose() const { Mat3f ret = *this; - std::swap(ret.x.y, ret.y.x); - std::swap(ret.x.z, ret.z.x); - std::swap(ret.y.z, ret.z.y); + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); return ret; } @@ -223,3 +225,10 @@ Vector3f operator/(const Vector3f &vec, const float scalar) vecOut.z = vec.z / scalar; return vecOut; } + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} From f0f28d5420f79e41b89c7519be239c246d31c4d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 08:35:11 -0700 Subject: [PATCH 282/493] POSIX SIM: Reset the HRT on system boot --- src/modules/simulator/simulator_mavlink.cpp | 4 ++++ src/platforms/posix/px4_layer/drv_hrt.c | 8 ++++++++ 2 files changed, 12 insertions(+) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 52f595cd43..7b6b4594f0 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -40,6 +40,8 @@ #include #include +extern "C" __EXPORT hrt_abstime hrt_reset(void); + #define SEND_INTERVAL 20 #define UDP_PORT 14560 #define PIXHAWK_DEVICE "/dev/ttyACM0" @@ -406,6 +408,8 @@ void Simulator::pollForMAVLinkMessages(bool publish) } PX4_WARN("Found initial message, pret = %d",pret); _initialized = true; + // reset system time + (void)hrt_reset(); if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 0a45644481..2fbe8cd70a 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -66,6 +66,8 @@ static hrt_abstime px4_timestart = 0; static void hrt_call_invoke(void); +__EXPORT hrt_abstime hrt_reset(void); + static void hrt_lock(void) { //printf("hrt_lock\n"); @@ -128,6 +130,12 @@ hrt_abstime hrt_absolute_time(void) return ts_to_abstime(&ts) - px4_timestart; } +__EXPORT hrt_abstime hrt_reset(void) +{ + px4_timestart = 0; + return hrt_absolute_time(); +} + /* * Convert a timespec to absolute time. */ From dac74db104f8fda5bd3b8b68f3b7f45878a27865 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 17:53:19 +0200 Subject: [PATCH 283/493] change the nested if structure which tries all i2c busses to a loop. --- src/drivers/px4flow/px4flow.cpp | 81 ++++++++++++++++++--------------- 1 file changed, 45 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 2166258c15..8bf20290ca 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -616,65 +616,74 @@ start() errx(1, "already started"); } - /* create the driver */ - g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - + const int busses_to_try[] = { + PX4_I2C_BUS_EXPANSION, #ifdef PX4_I2C_BUS_ESC - delete g_dev; - /* try 2nd bus */ - g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); + PX4_I2C_BUS_ESC, + #endif + PX4_I2C_BUS_ONBOARD, + -1 + }; + const int *cur_bus = busses_to_try; + while(*cur_bus != -1) { + /* create the driver */ + //warnx("trying bus %d", *cur_bus); + g_dev = new PX4FLOW(*cur_bus); if (g_dev == nullptr) { - goto fail; + /* this is a fatal error */ + break; } - - if (OK != g_dev->init()) { - #endif - - delete g_dev; - /* try 3rd bus */ - g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - #ifdef PX4_I2C_BUS_ESC + + /* init the driver: */ + if (OK == g_dev->init()) { + /* success! */ + break; } - #endif + + /* destroy it again because it failed. */ + delete g_dev; + g_dev = nullptr; + + /* try next! */ + cur_bus++; } - + + /* check whether we found it: */ + if (*cur_bus == -1) { + goto not_found; + } + + /* check for failure: */ + if (g_dev == nullptr) { + goto fatal_fail; + } + /* set the poll rate to default, starts automatic data collection */ fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - goto fail; + goto fatal_fail; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - goto fail; + goto fatal_fail; } exit(0); -fail: +not_found: + /* for now we do the same as if there was a fatal failure. */ + warnx("PX4FLOW not found on I2C busses"); + +fatal_fail: if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - errx(1, "no PX4FLOW connected over I2C"); + errx(1, "PX4FLOW could not be started over I2C"); } /** From d9e6cb0f584f6e4016688a12a03e641af47cff4a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 18:28:19 +0200 Subject: [PATCH 284/493] implemented retrying the connection to the px4flow sensor before giving up. --- src/drivers/px4flow/px4flow.cpp | 133 ++++++++++++++++++-------------- 1 file changed, 76 insertions(+), 57 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 8bf20290ca..8ee365d8df 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -596,7 +596,11 @@ namespace px4flow #endif const int ERROR = -1; -PX4FLOW *g_dev; +PX4FLOW *g_dev = nullptr; +bool start_in_progress = false; + +const int START_RETRY_COUNT = 5; +const int START_RETRY_TIMEOUT = 1000; void start(); void stop(); @@ -611,78 +615,93 @@ void start() { int fd; + + /* entry check: */ + if (start_in_progress) { + errx(1, "start in progress"); + } + start_in_progress = true; if (g_dev != nullptr) { + start_in_progress = false; errx(1, "already started"); } - const int busses_to_try[] = { - PX4_I2C_BUS_EXPANSION, - #ifdef PX4_I2C_BUS_ESC - PX4_I2C_BUS_ESC, - #endif - PX4_I2C_BUS_ONBOARD, - -1 - }; + int retry_nr = 0; + while (1) { + const int busses_to_try[] = { + PX4_I2C_BUS_EXPANSION, + #ifdef PX4_I2C_BUS_ESC + PX4_I2C_BUS_ESC, + #endif + PX4_I2C_BUS_ONBOARD, + -1 + }; - const int *cur_bus = busses_to_try; - while(*cur_bus != -1) { - /* create the driver */ - //warnx("trying bus %d", *cur_bus); - g_dev = new PX4FLOW(*cur_bus); - if (g_dev == nullptr) { - /* this is a fatal error */ - break; + const int *cur_bus = busses_to_try; + + while(*cur_bus != -1) { + /* create the driver */ + /* warnx("trying bus %d", *cur_bus); */ + g_dev = new PX4FLOW(*cur_bus); + if (g_dev == nullptr) { + /* this is a fatal error */ + break; + } + + /* init the driver: */ + if (OK == g_dev->init()) { + /* success! */ + break; + } + + /* destroy it again because it failed. */ + delete g_dev; + g_dev = nullptr; + + /* try next! */ + cur_bus++; } - - /* init the driver: */ - if (OK == g_dev->init()) { + + /* check whether we found it: */ + if (*cur_bus != -1) { + + /* check for failure: */ + if (g_dev == nullptr) { + break; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + break; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { + break; + } + /* success! */ + start_in_progress = false; + exit(0); + } + + if (retry_nr < START_RETRY_COUNT) { + warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr); + usleep(START_RETRY_TIMEOUT * 1000); + retry_nr++; + } else { break; } - - /* destroy it again because it failed. */ - delete g_dev; - g_dev = nullptr; - - /* try next! */ - cur_bus++; } - /* check whether we found it: */ - if (*cur_bus == -1) { - goto not_found; - } - - /* check for failure: */ - if (g_dev == nullptr) { - goto fatal_fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fatal_fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - goto fatal_fail; - } - - exit(0); - -not_found: - /* for now we do the same as if there was a fatal failure. */ - warnx("PX4FLOW not found on I2C busses"); - -fatal_fail: - if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - + + start_in_progress = false; errx(1, "PX4FLOW could not be started over I2C"); } From 440aedebadc3db8ee750736ab297b25a4c04ec4a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 21:10:48 +0200 Subject: [PATCH 285/493] change start script to launch the px4flow driver in background. Fixes issue #2145 --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 474db36ef7..e5d527dd71 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -111,7 +111,7 @@ else fi # Check for flow sensor -if px4flow start +if px4flow start & then fi From cf8307f039f328a42afc719fe1a74492c7edb527 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 18:57:59 +0200 Subject: [PATCH 286/493] Commander: Low-pass battery throttle to better match battery dynamics --- src/modules/commander/commander_helper.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 68949eec1d..491ce32a5c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -108,6 +108,7 @@ static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static unsigned int counter = 0; +static float throttle_lowpassed = 0.0f; int battery_init() { @@ -381,8 +382,15 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; + // XXX this time constant needs to become tunable + // but really, the right fix are smart batteries. + float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f; + if (isfinite(val)) { + throttle_lowpassed = val; + } + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed); /* the range from full to empty is the same for batteries under load and without load, * since the voltage drop applies to both the full and empty state */ From 6e0aa90bb832d4cf9a0d5e3619914f91109cbe7b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 18:57:59 +0200 Subject: [PATCH 287/493] Commander: Low-pass battery throttle to better match battery dynamics --- src/modules/commander/commander_helper.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 68949eec1d..491ce32a5c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -108,6 +108,7 @@ static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static unsigned int counter = 0; +static float throttle_lowpassed = 0.0f; int battery_init() { @@ -381,8 +382,15 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; + // XXX this time constant needs to become tunable + // but really, the right fix are smart batteries. + float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f; + if (isfinite(val)) { + throttle_lowpassed = val; + } + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed); /* the range from full to empty is the same for batteries under load and without load, * since the voltage drop applies to both the full and empty state */ From a74cc5bf494b05f48e796190d3ea3e43c849aa31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 19:07:08 +0200 Subject: [PATCH 288/493] MAVLink app: Fix scaling of battery current --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0b6dfcab5f..cb56331e95 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -537,7 +537,7 @@ protected: msg.onboard_control_sensors_health = status.onboard_control_sensors_health; msg.load = status.load * 1000.0f; msg.voltage_battery = status.battery_voltage * 1000.0f; - msg.current_battery = status.battery_current * 100.0f; + msg.current_battery = status.battery_current / 10.0f; msg.drop_rate_comm = status.drop_rate_comm; msg.errors_comm = status.errors_comm; msg.errors_count1 = status.errors_count1; @@ -562,7 +562,7 @@ protected: bat_msg.voltages[i] = 0; } } - bat_msg.current_battery = status.battery_current * 100.0f; + bat_msg.current_battery = status.battery_current / 10.0f; bat_msg.current_consumed = status.battery_discharged_mah; bat_msg.energy_consumed = -1.0f; bat_msg.battery_remaining = (status.battery_voltage > 0) ? From a4a73ff53edfafbee8a58c0a5e43971936eb27e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 11:02:19 +0200 Subject: [PATCH 289/493] POSIX: Be less verbose on CXX builds, add option to provide log verbosity level on commandline --- makefiles/posix/toolchain_native.mk | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/makefiles/posix/toolchain_native.mk b/makefiles/posix/toolchain_native.mk index f58e55314b..b34038b5c3 100644 --- a/makefiles/posix/toolchain_native.mk +++ b/makefiles/posix/toolchain_native.mk @@ -44,6 +44,12 @@ # Set to 1 for GCC-4.8.2 and to 0 for Clang-3.5 (Ubuntu 14.04) USE_GCC?=0 +ifeq ($(PX4_DEBUG_LEVEL),) +VERBOSITY_LEVEL= +else +VERBOSITY_LEVEL=$(PX4_DEBUG_LEVEL) +endif + ifneq ($(USE_GCC),1) HAVE_CLANG35:=$(shell clang-3.5 -dumpversion 2>/dev/null) @@ -121,6 +127,7 @@ $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -Dnoreturn_function=__attribute__\(\(noreturn\)\) \ + -D$(VERBOSITY_LEVEL) \ -I$(PX4_BASE)/src/modules/systemlib \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/posix/include \ @@ -296,7 +303,7 @@ endef define COMPILEXX @$(ECHO) "CXX: $1" @$(MKDIR) -p $(dir $2) - @echo $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 + @$(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 endef From a02319e90109449a61042df9d25471f7a76c2af7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 11:03:27 +0200 Subject: [PATCH 290/493] PX4 log: Fix formatting for debug and trace builds --- src/platforms/px4_log.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 21f17ce68e..153985efa9 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -94,7 +94,7 @@ __EXPORT extern int __px4_log_level_current; #define __px4__log_level_fmt "%-5s " #define __px4__log_level_arg(level) ,__px4_log_level_str[level] #define __px4__log_thread_fmt "%#X " -#define __px4__log_thread_arg ,pthread_self() +#define __px4__log_thread_arg ,(unsigned int)pthread_self() #define __px4__log_file_and_line_fmt " (file %s line %u)" #define __px4__log_file_and_line_arg , __FILE__, __LINE__ @@ -275,7 +275,7 @@ __EXPORT extern int __px4_log_level_current; #define PX4_PANIC(FMT, ...) __px4_log_timestamp_thread_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) #define PX4_ERR(FMT, ...) __px4_log_timestamp_thread_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) #define PX4_WARN(FMT, ...) __px4_log_timestamp_thread_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) -#define PX4_DEBUG(FMT, ...) __px4_log_timestamp_thread(_PX4_LOG_LEVEL_DEBUG, FMT, __VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_timestamp_thread(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #elif defined(DEBUG_BUILD) /**************************************************************************** @@ -284,7 +284,7 @@ __EXPORT extern int __px4_log_level_current; #define PX4_PANIC(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) #define PX4_ERR(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) #define PX4_WARN(FMT, ...) __px4_log_timestamp_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) -#define PX4_DEBUG(FMT, ...) __px4_log_timestamp(_PX4_LOG_LEVEL_DEBUG, FMT, __VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_timestamp(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #elif defined(RELEASE_BUILD) /**************************************************************************** From 65d035a89292c05db61493e5fdde35b3eaafca01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 12:05:59 +0200 Subject: [PATCH 291/493] Camera trigger: fix formatting --- src/modules/camera_trigger/camera_trigger.cpp | 167 +++++++++--------- 1 file changed, 79 insertions(+), 88 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index bfbd770c8b..c4d829047e 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -89,21 +89,21 @@ public: * Display info. */ void info(); - + int pin; - + private: - + struct hrt_call _pollcall; struct hrt_call _firecall; - + int _gpio_fd; int _polarity; float _activation_time; float _integration_time; float _transfer_time; - uint32_t _trigger_seq; + uint32_t _trigger_seq; bool _trigger_enabled; int _sensor_sub; @@ -116,10 +116,10 @@ private: struct vehicle_command_s _command; param_t polarity ; - param_t activation_time ; + param_t activation_time ; param_t integration_time ; param_t transfer_time ; - + /** * Topic poller to check for fire info. */ @@ -162,13 +162,13 @@ CameraTrigger::CameraTrigger() : memset(&_trigger, 0, sizeof(_trigger)); memset(&_sensor, 0, sizeof(_sensor)); memset(&_command, 0, sizeof(_command)); - + memset(&_pollcall, 0, sizeof(_pollcall)); - memset(&_firecall, 0, sizeof(_firecall)); + memset(&_firecall, 0, sizeof(_firecall)); // Parameters polarity = param_find("TRIG_POLARITY"); - activation_time = param_find("TRIG_ACT_TIME"); + activation_time = param_find("TRIG_ACT_TIME"); integration_time = param_find("TRIG_INT_TIME"); transfer_time = param_find("TRIG_TRANS_TIME"); } @@ -184,37 +184,37 @@ CameraTrigger::start() _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); - - param_get(polarity, &_polarity); - param_get(activation_time, &_activation_time); - param_get(integration_time, &_integration_time); - param_get(transfer_time, &_transfer_time); - + + param_get(polarity, &_polarity); + param_get(activation_time, &_activation_time); + param_get(integration_time, &_integration_time); + param_get(transfer_time, &_transfer_time); + stm32_configgpio(GPIO_GPIO0_OUTPUT); - if(_polarity == 0) { + if (_polarity == 0) { stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); // GPIO pin pull high - } - else if(_polarity == 1) { + + } else if (_polarity == 1) { stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); // GPIO pin pull low - } - else { + + } else { warnx(" invalid trigger polarity setting. stopping."); stop(); } - + poll(this); // Trampoline call } void CameraTrigger::stop() -{ +{ hrt_cancel(&_firecall); hrt_cancel(&_pollcall); if (camera_trigger::g_camera_trigger != nullptr) { - delete (camera_trigger::g_camera_trigger); + delete(camera_trigger::g_camera_trigger); } } @@ -226,60 +226,55 @@ CameraTrigger::poll(void *arg) bool updated; orb_check(trig->_vcommand_sub, &updated); - + if (updated) { - + orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command); - - if(trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) - { - if(trig->_command.param1 < 1) - { - if(trig->_trigger_enabled) - { - trig->_trigger_enabled = false ; + + if (trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { + if (trig->_command.param1 < 1) { + if (trig->_trigger_enabled) { + trig->_trigger_enabled = false ; } - } - else if(trig->_command.param1 >= 1) - { - if(!trig->_trigger_enabled) - { + + } else if (trig->_command.param1 >= 1) { + if (!trig->_trigger_enabled) { trig->_trigger_enabled = true ; - } + } } // Set trigger rate from command - if(trig->_command.param2 > 0) - { + if (trig->_command.param2 > 0) { trig->_integration_time = trig->_command.param2; param_set(trig->integration_time, &(trig->_integration_time)); - } + } } } - if(!trig->_trigger_enabled) { - hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig); + if (!trig->_trigger_enabled) { + hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig); return; - } - else - { + + } else { engage(trig); - hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); - + hrt_call_after(&trig->_firecall, trig->_activation_time * 1000, (hrt_callout)&CameraTrigger::disengage, trig); + orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); - + trig->_trigger.timestamp = trig->_sensor.timestamp; // get IMU timestamp trig->_trigger.seq = trig->_trigger_seq++; if (trig->_trigger_pub != nullptr) { orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger); + } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } - hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000, (hrt_callout)&CameraTrigger::poll, trig); + hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time) * 1000, + (hrt_callout)&CameraTrigger::poll, trig); } - + } void @@ -289,17 +284,15 @@ CameraTrigger::engage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); stm32_configgpio(GPIO_GPIO0_OUTPUT); - - if(trig->_polarity == 0) // ACTIVE_LOW - { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); + + if (trig->_polarity == 0) { // ACTIVE_LOW + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); + + } else if (trig->_polarity == 1) { // ACTIVE_HIGH + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); } - else if(trig->_polarity == 1) // ACTIVE_HIGH - { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); - } - - + + } void @@ -307,18 +300,16 @@ CameraTrigger::disengage(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - + stm32_configgpio(GPIO_GPIO0_OUTPUT); - - if(trig->_polarity == 0) // ACTIVE_LOW - { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); + + if (trig->_polarity == 0) { // ACTIVE_LOW + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); + + } else if (trig->_polarity == 1) { // ACTIVE_HIGH + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); } - else if(trig->_polarity == 1) // ACTIVE_HIGH - { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); - } - + } void @@ -333,8 +324,8 @@ CameraTrigger::info() static void usage() { errx(1, "usage: camera_trigger {start|stop|info} [-p ]\n" - "\t-p \tUse specified AUX OUT pin number (default: 1)" - ); + "\t-p \tUse specified AUX OUT pin number (default: 1)" + ); } int camera_trigger_main(int argc, char *argv[]) @@ -348,25 +339,26 @@ int camera_trigger_main(int argc, char *argv[]) if (camera_trigger::g_camera_trigger != nullptr) { errx(0, "already running"); } - + camera_trigger::g_camera_trigger = new CameraTrigger; if (camera_trigger::g_camera_trigger == nullptr) { errx(1, "alloc failed"); } - + if (argc > 3) { - + camera_trigger::g_camera_trigger->pin = (int)argv[3]; - if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) { - warnx("starting trigger on pin : %li ", atoi(argv[3])); + + if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) { + warnx("starting trigger on pin : %li ", atoi(argv[3])); camera_trigger::g_camera_trigger->pin = atoi(argv[3]); - } - else - { - usage(); + + } else { + usage(); } } + camera_trigger::g_camera_trigger->start(); return 0; @@ -377,11 +369,10 @@ int camera_trigger_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { - camera_trigger::g_camera_trigger->stop(); + camera_trigger::g_camera_trigger->stop(); - } - else if (!strcmp(argv[1], "info")) { - camera_trigger::g_camera_trigger->info(); + } else if (!strcmp(argv[1], "info")) { + camera_trigger::g_camera_trigger->info(); } else { usage(); From 48c356fb2bbb7cdae338b4e4986a41c0e1a9278c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 12:06:45 +0200 Subject: [PATCH 292/493] Fix parallel build for POSIX / QuRT --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 88683c8941..ee9449a494 100644 --- a/Makefile +++ b/Makefile @@ -194,7 +194,7 @@ testbuild: nuttx posix posix-arm qurt: ifeq ($(GOALS),) - make PX4_TARGET_OS=$@ $(GOALS) + $(MAKE) PX4_TARGET_OS=$@ $(GOALS) else export PX4_TARGET_OS=$@ endif @@ -206,7 +206,7 @@ sitl_plane: $(Q) Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing qurtrun: - make PX4_TARGET_OS=qurt sim + $(MAKE) PX4_TARGET_OS=qurt sim # # Unittest targets. Builds and runs the host-level From 8c5d99484e80787164716d87b6e35c64730bb43d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 13:45:10 +0200 Subject: [PATCH 293/493] Navigator: Improve output --- src/modules/navigator/mission.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index da5021c7e8..45839064e4 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -265,7 +265,7 @@ Mission::update_offboard_mission() _navigator->set_mission_result_updated(); } else { - warnx("offboard mission update failed"); + PX4_WARN("offboard mission update failed"); } if (failed) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7b4bfccf33..1d252ea646 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -337,11 +337,12 @@ Navigator::task_main() if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ + PX4_WARN("timed out"); continue; } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ - warn("poll error %d, %d", pret, errno); + PX4_WARN("poll error %d, %d", pret, errno); continue; } From 7b8f7f7ac49eef5b23e040bb0702713396fb03c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 13:45:42 +0200 Subject: [PATCH 294/493] Posix tasks: Mark task creation --- src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index ac87ddfd55..5515ef9e9b 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -142,6 +142,8 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; + PX4_WARN("starting task %s", name); + rv = pthread_attr_init(&attr); if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); From 10a77a151319a5eda969b10c922e1bae0aa0b7dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 13:46:24 +0200 Subject: [PATCH 295/493] Dataman: Be more verbose on error --- src/modules/dataman/dataman.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 20393ea250..1594e340c8 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -664,6 +664,7 @@ task_main(int argc, char *argv[]) int file_size = lseek(g_task_fd, 0, SEEK_END); if ((file_size % k_sector_size) != 0) { warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path); + warnx("Size: %u, sector size: %d", file_size, k_sector_size); close(g_task_fd); unlink(k_data_manager_device_path); } else { From f689321dedbdb84fd8dd1e19e33466f489ba2d69 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 13:46:49 +0200 Subject: [PATCH 296/493] POSIX: HRT: Be more verbose on error --- src/platforms/posix/px4_layer/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 2fbe8cd70a..f51802d98b 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -351,7 +351,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte sq_rem(&entry->link, &callout_queue); } -#if 0 +#if 1 // Use this to debug busy CPU that keeps rescheduling with 0 period time if (interval < HRT_INTERVAL_MIN) { From 9bd5c6ef6ece6378f8dfdffe54720c3a4682448f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 13:47:33 +0200 Subject: [PATCH 297/493] Travis CI: Add CLANG so we can start to phase in SITL tests --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 7fd0aeeffc..134cdcca67 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,6 +18,7 @@ addons: - build-essential - ccache - cmake + - clang-3.5 - g++-4.8 - gcc-4.8 - genromfs From 2adb48ce9004bff96fc6e2508f30ecdb49b398a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 13:48:07 +0200 Subject: [PATCH 298/493] MC pos control: Better default velocity gain. --- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 501cd695b8..a3793b0cc0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -79,7 +79,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); * @min 0.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); /** * Integral gain for vertical velocity error From 01fc57035187a37f8962dc9a404d5f23ca3443fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 14:01:17 +0200 Subject: [PATCH 299/493] POSIX: Fix build for non-trace builds --- makefiles/posix/toolchain_native.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/posix/toolchain_native.mk b/makefiles/posix/toolchain_native.mk index b34038b5c3..088e123cb6 100644 --- a/makefiles/posix/toolchain_native.mk +++ b/makefiles/posix/toolchain_native.mk @@ -47,7 +47,7 @@ USE_GCC?=0 ifeq ($(PX4_DEBUG_LEVEL),) VERBOSITY_LEVEL= else -VERBOSITY_LEVEL=$(PX4_DEBUG_LEVEL) +VERBOSITY_LEVEL=-D$(PX4_DEBUG_LEVEL) endif ifneq ($(USE_GCC),1) @@ -127,7 +127,7 @@ $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -Dnoreturn_function=__attribute__\(\(noreturn\)\) \ - -D$(VERBOSITY_LEVEL) \ + $(VERBOSITY_LEVEL)\ -I$(PX4_BASE)/src/modules/systemlib \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/posix/include \ From 1d06f3ed5683c83f8a6b4ee6d26500bc68790787 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 15:08:24 +0200 Subject: [PATCH 300/493] Add Vagrant config --- Vagrantfile | 95 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 95 insertions(+) create mode 100644 Vagrantfile diff --git a/Vagrantfile b/Vagrantfile new file mode 100644 index 0000000000..d600820b05 --- /dev/null +++ b/Vagrantfile @@ -0,0 +1,95 @@ +# -*- mode: ruby -*- +# vi: set ft=ruby : + +# All Vagrant configuration is done below. The "2" in Vagrant.configure +# configures the configuration version (we support older styles for +# backwards compatibility). Please don't change it unless you know what +# you're doing. +Vagrant.configure(2) do |config| + + # Every Vagrant development environment requires a box. You can search for + # boxes at https://atlas.hashicorp.com/search. + config.vm.box = "ubuntu/trusty64" + + # Disable automatic box update checking. If you disable this, then + # boxes will only be checked for updates when the user runs + # `vagrant box outdated`. This is not recommended. + # config.vm.box_check_update = false + + # Create a forwarded port mapping which allows access to a specific port + # within the machine from a port on the host machine. In the example below, + # accessing "localhost:8080" will access port 80 on the guest machine. + # MAVLink telemetry via UDP in SITL mode + config.vm.network "forwarded_port", guest: 14556, host: 14556, protocol: "udp" + # SITL simulation data + config.vm.network "forwarded_port", guest: 14560, host: 14560, protocol: "udp" + + # Create a private network, which allows host-only access to the machine + # using a specific IP. + # config.vm.network "private_network", ip: "192.168.33.10" + + # Create a public network, which generally matched to bridged network. + # Bridged networks make the machine appear as another physical device on + # your network. + # config.vm.network "public_network" + + # Share an additional folder to the guest VM. The first argument is + # the path on the host to the actual folder. The second argument is + # the path on the guest to mount the folder. And the optional third + # argument is a set of non-required options. + config.vm.synced_folder ".", "/Firmware" + + # Provider-specific configuration so you can fine-tune various + # backing providers for Vagrant. These expose provider-specific options. + # Example for VirtualBox: + # + config.vm.provider "virtualbox" do |vb| + # Display the VirtualBox GUI when booting the machine + vb.gui = false + vb.customize ["modifyvm", :id, "--ioapic", "on"] + vb.customize ["modifyvm", :id, "--cpus", "2"] + + # Customize the amount of memory on the VM: + vb.memory = "2048" + end + # + # View the documentation for the provider you are using for more + # information on available options. + + # Define a Vagrant Push strategy for pushing to Atlas. Other push strategies + # such as FTP and Heroku are also available. See the documentation at + # https://docs.vagrantup.com/v2/push/atlas.html for more information. + # config.push.define "atlas" do |push| + # push.app = "YOUR_ATLAS_USERNAME/YOUR_APPLICATION_NAME" + # end + + # Enable provisioning with a shell script. Additional provisioners such as + # Puppet, Chef, Ansible, Salt, and Docker are also available. Please see the + # documentation for more information about their specific syntax and use. + config.vm.provision "shell", inline: <<-SHELL + # Ensure we start in the Firmware folder + echo "cd /Firmware" >> ~/.bashrc + # Install software + sudo apt-get update + sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core + pushd . + cd ~ + wget -q https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 + tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 + exportline="export PATH=$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:\$PATH" + if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi + . ~/.profile + popd + # setup ccache + mkdir -p ~/bin + ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ + ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc + ln -s /usr/bin/ccache ~/bin/g++-4.8 + ln -s /usr/bin/ccache ~/bin/gcc-4.8 + export PATH=~/bin:$PATH + + # Configure hardware related bits + sudo apt-get -y remove modemmanager + sudo usermod -a -G dialout $USER + SHELL +end From 936749632be0f139fff386afdf7ec9adcd3c9b22 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 15:08:40 +0200 Subject: [PATCH 301/493] POSIX: Add GDB init --- Tools/posix.gdbinit | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 Tools/posix.gdbinit diff --git a/Tools/posix.gdbinit b/Tools/posix.gdbinit new file mode 100644 index 0000000000..0de1123362 --- /dev/null +++ b/Tools/posix.gdbinit @@ -0,0 +1,2 @@ +handle SIGCONT nostop +run From 0f24429d32e1f5e3d183c33f9cf8d24a9b86aa65 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 16:15:51 +0200 Subject: [PATCH 302/493] Vagrant: Force time sync --- Vagrantfile | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Vagrantfile b/Vagrantfile index d600820b05..25ba1ce35c 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -49,6 +49,13 @@ Vagrant.configure(2) do |config| vb.customize ["modifyvm", :id, "--ioapic", "on"] vb.customize ["modifyvm", :id, "--cpus", "2"] + # Since make and other tools freak out if they see timestamps + # from the future and we share directories, tightly lock the host and guest clocks together (clock sync if more than 2 seconds off) + vb.customize ["guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-threshold", 2000] + # Do this on start and restore + vb.customize ["guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-start"] + vb.customize ["guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-on-restore", "1"] + # Customize the amount of memory on the VM: vb.memory = "2048" end From 2314ebe4c25dcc14e07662d5dbe17401f353ac8c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 16:16:09 +0200 Subject: [PATCH 303/493] POSIX Makefile: Fix CLANG 3.5 --- makefiles/posix/toolchain_native.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/posix/toolchain_native.mk b/makefiles/posix/toolchain_native.mk index 088e123cb6..8b07a35f88 100644 --- a/makefiles/posix/toolchain_native.mk +++ b/makefiles/posix/toolchain_native.mk @@ -55,7 +55,7 @@ ifneq ($(USE_GCC),1) HAVE_CLANG35:=$(shell clang-3.5 -dumpversion 2>/dev/null) # Clang will report 4.2.1 as GCC version -HAVE_CLANG:=$(shell clang -dumpversion) +HAVE_CLANG:=$(shell clang -dumpversion 2> /dev/null) #If using ubuntu 14.04 and packaged clang 3.5 ifeq ($(HAVE_CLANG35),4.2.1) From fc9d6ac39b17b9c78f6ff7a1cc3d5811ca10ea3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 16:16:57 +0200 Subject: [PATCH 304/493] POSIX baro SITL: Failed advert type is pointer, not number --- src/platforms/posix/drivers/barosim/baro.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index a70e394213..8958fedfe4 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -278,7 +278,7 @@ BAROSIM::init() _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); - if (_baro_topic == (orb_advert_t)(-1)) { + if (_baro_topic == nullptr) { PX4_ERR("failed to create sensor_baro publication"); } From a45391b244513ef2fe2d1e90246b92e1ba5f933a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jul 2015 16:17:29 +0200 Subject: [PATCH 305/493] Navigator: If orb copy fails, print FD --- src/modules/navigator/mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 45839064e4..ea515e62ad 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -265,7 +265,7 @@ Mission::update_offboard_mission() _navigator->set_mission_result_updated(); } else { - PX4_WARN("offboard mission update failed"); + PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub()); } if (failed) { From fd63ba7b898b95469b62f6f3f42f12e042aae619 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jul 2015 01:43:00 +0200 Subject: [PATCH 306/493] FW pos control: Widen acceptance range for yaw rate to re-engage heading hold --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b5436602ec..8856dd829d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -97,7 +97,7 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode #define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane -#define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode +#define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading #define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) From 5f586fc354caf94facaf79ca5702ddc57ef4a982 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:51:10 +0200 Subject: [PATCH 307/493] Mixer library: Fix code style --- src/modules/px4iofirmware/mixer.cpp | 35 ++++-- src/modules/systemlib/mixer/mixer.cpp | 24 +++-- src/modules/systemlib/mixer/mixer.h | 34 +++--- src/modules/systemlib/mixer/mixer_load.c | 16 ++- .../systemlib/mixer/mixer_multirotor.cpp | 102 ++++++++++-------- src/modules/systemlib/mixer/mixer_simple.cpp | 42 +++++--- 6 files changed, 157 insertions(+), 96 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0106fa1eb7..a2cfdf491b 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -35,6 +35,8 @@ * @file mixer.cpp * * Control channel input/output mixer and failsafe. + * + * @author Lorenz Meier */ #include @@ -64,6 +66,7 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; static bool should_arm = false; +static bool should_arm_nothrottle = false; static bool should_always_enable_pwm = false; static volatile bool in_mixer = false; @@ -172,6 +175,11 @@ mixer_tick(void) ) ); + should_arm_nothrottle = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); @@ -237,24 +245,25 @@ mixer_tick(void) /* the pwm limit call takes care of out of band errors */ pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + /* clamp unused outputs to zero */ + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = 0; + outputs[i] = 0; + } + /* store normalized outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); } } /* set arming */ - bool needs_to_arm = (should_arm || should_always_enable_pwm); + bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm); /* check any conditions that prevent arming */ if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { needs_to_arm = false; } - if (!should_arm && !should_always_enable_pwm) { - needs_to_arm = false; - } if (needs_to_arm && !mixer_servos_armed) { /* need to arm, but not armed */ @@ -308,8 +317,9 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group >= PX4IO_CONTROL_GROUPS) + if (control_group >= PX4IO_CONTROL_GROUPS) { return -1; + } switch (source) { case MIX_FMU: @@ -359,8 +369,8 @@ mixer_callback(uintptr_t handle, } } - /* motor spinup phase - lock throttle to zero */ - if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + /* motor spinup phase or only safety off, but not armed - lock throttle to zero */ + if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_THROTTLE) { /* limit the throttle output to zero during motor spinup, @@ -458,8 +468,9 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ - if (resid > 0) + if (resid > 0) { memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + } mixer_text_length = resid; @@ -482,8 +493,9 @@ mixer_set_failsafe() */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { return; + } /* set failsafe defaults to the values for all inputs = 0 */ float outputs[PX4IO_SERVO_COUNT]; @@ -501,7 +513,8 @@ mixer_set_failsafe() } /* disable the rest of the outputs */ - for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servo_failsafe[i] = 0; + } } diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 3ab41c5c58..cd2010b92c 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -98,20 +98,25 @@ Mixer::scale(const mixer_scaler_s &scaler, float input) int Mixer::scale_check(struct mixer_scaler_s &scaler) { - if (scaler.offset > 1.001f) + if (scaler.offset > 1.001f) { return 1; + } - if (scaler.offset < -1.001f) + if (scaler.offset < -1.001f) { return 2; + } - if (scaler.min_output > scaler.max_output) + if (scaler.min_output > scaler.max_output) { return 3; + } - if (scaler.min_output < -1.001f) + if (scaler.min_output < -1.001f) { return 4; + } - if (scaler.max_output > 1.001f) + if (scaler.max_output > 1.001f) { return 5; + } return 0; } @@ -120,11 +125,14 @@ const char * Mixer::findtag(const char *buf, unsigned &buflen, char tag) { while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) + if ((buf[0] == tag) && (buf[1] == ':')) { return buf; + } + buf++; buflen--; } + return nullptr; } @@ -174,13 +182,15 @@ NullMixer::from_text(const char *buf, unsigned &buflen) /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') + if (buf[i] == '\0') { continue; + } /* require a space or newline at the end of the buffer, fail on printable chars */ if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { /* found a line ending or space, so no split symbols / numbers. good. */ break; + } else { return nm; } diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 1190683015..cd98141905 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -222,7 +222,7 @@ protected: * @param buflen length of the buffer. * @param tag character to search for. */ - static const char * findtag(const char *buf, unsigned &buflen, char tag); + static const char *findtag(const char *buf, unsigned &buflen, char tag); /** * Skip a line @@ -231,13 +231,13 @@ protected: * @param buflen length of the buffer. * @return 0 / OK if a line could be skipped, 1 else */ - static const char * skipline(const char *buf, unsigned &buflen); + static const char *skipline(const char *buf, unsigned &buflen); private: /* do not allow to copy due to prt data members */ - Mixer(const Mixer&); - Mixer& operator=(const Mixer&); + Mixer(const Mixer &); + Mixer &operator=(const Mixer &); }; /** @@ -316,8 +316,8 @@ private: Mixer *_first; /**< linked list of mixers */ /* do not allow to copy due to pointer data members */ - MixerGroup(const MixerGroup&); - MixerGroup operator=(const MixerGroup&); + MixerGroup(const MixerGroup &); + MixerGroup operator=(const MixerGroup &); }; /** @@ -437,8 +437,8 @@ private: uint8_t &control_index); /* do not allow to copy due to ptr data members */ - SimpleMixer(const SimpleMixer&); - SimpleMixer operator=(const SimpleMixer&); + SimpleMixer(const SimpleMixer &); + SimpleMixer operator=(const SimpleMixer &); }; /** @@ -449,13 +449,13 @@ private: typedef unsigned int MultirotorGeometryUnderlyingType; enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; -/** - * Multi-rotor mixer for pre-defined vehicle geometries. - * - * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to - * a set of outputs based on the configured geometry. - */ -class __EXPORT MultirotorMixer : public Mixer + /** + * Multi-rotor mixer for pre-defined vehicle geometries. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to + * a set of outputs based on the configured geometry. + */ + class __EXPORT MultirotorMixer : public Mixer { public: /** @@ -531,8 +531,8 @@ private: const Rotor *_rotors; /* do not allow to copy due to ptr data members */ - MultirotorMixer(const MultirotorMixer&); - MultirotorMixer operator=(const MultirotorMixer&); + MultirotorMixer(const MultirotorMixer &); + MultirotorMixer operator=(const MultirotorMixer &); }; #endif diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index 0d629d6100..c0950d77a7 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -52,6 +52,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* open the mixer definition file */ fp = fopen(fname, "r"); + if (fp == NULL) { warnx("file not found"); return -1; @@ -59,29 +60,38 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* read valid lines from the file into a buffer */ buf[0] = '\0'; + for (;;) { /* get a line, bail on error/EOF */ line[0] = '\0'; - if (fgets(line, sizeof(line), fp) == NULL) + + if (fgets(line, sizeof(line), fp) == NULL) { break; + } /* if the line doesn't look like a mixer definition line, skip it */ - if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) { continue; + } /* compact whitespace in the buffer */ char *t, *f; + for (f = line; *f != '\0'; f++) { /* scan for space characters */ if (*f == ' ') { /* look for additional spaces */ t = f + 1; - while (*t == ' ') + + while (*t == ' ') { t++; + } + if (*t == '\0') { /* strip trailing whitespace */ *f = '\0'; + } else if (t > (f + 1)) { memmove(f + 1, t, strlen(t) + 1); } diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index aa0a8e7418..6bbc349d87 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -109,15 +109,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') + if (buf[i] == '\0') { continue; + } /* require a space or newline at the end of the buffer, fail on printable chars */ if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { /* found a line ending or space, so no split symbols / numbers. good. */ break; + } else { - debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); + debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen - 1, buf[i]); return nullptr; } @@ -134,6 +136,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); return nullptr; @@ -170,7 +173,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "8x")) { geometry = MultirotorGeometry::OCTA_X; - + } else if (!strcmp(geomname, "8c")) { geometry = MultirotorGeometry::OCTA_COX; @@ -222,6 +225,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) if (status_reg != NULL) { (*status_reg) = 0; } + // thrust boost parameters float thrust_increase_factor = 1.5f; float thrust_decrease_factor = 0.6f; @@ -238,6 +242,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) if (out < min_out) { min_out = out; } + if (out > max_out) { max_out = out; } @@ -248,87 +253,94 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) float boost = 0.0f; // value added to demanded thrust (can also be negative) float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch - if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { + if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; - if(max_thrust_diff >= -min_out) { + + if (max_thrust_diff >= -min_out) { boost = -min_out; - } - else { + + } else { boost = max_thrust_diff; - roll_pitch_scale = (thrust + boost)/(thrust - min_out); + roll_pitch_scale = (thrust + boost) / (thrust - min_out); } - } - else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { - float max_thrust_diff = thrust - thrust_decrease_factor*thrust; - if(max_thrust_diff >= max_out - 1.0f) { + + } else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { + float max_thrust_diff = thrust - thrust_decrease_factor * thrust; + + if (max_thrust_diff >= max_out - 1.0f) { boost = -(max_out - 1.0f); + } else { boost = -max_thrust_diff; - roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); + roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); } - } - else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { + + } else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; - boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff); - roll_pitch_scale = (thrust + boost)/(thrust - min_out); - } - else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) { - float max_thrust_diff = thrust - thrust_decrease_factor*thrust; - boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f); - roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); - } - else if (min_out < 0.0f && max_out > 1.0f) { - boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust); - roll_pitch_scale = (thrust + boost)/(thrust - min_out); + boost = constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff); + roll_pitch_scale = (thrust + boost) / (thrust - min_out); + + } else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) { + float max_thrust_diff = thrust - thrust_decrease_factor * thrust; + boost = constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f); + roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); + + } else if (min_out < 0.0f && max_out > 1.0f) { + boost = constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust, + thrust_increase_factor * thrust - thrust); + roll_pitch_scale = (thrust + boost) / (thrust - min_out); } // notify if saturation has occurred - if(min_out < 0.0f) { - if(status_reg != NULL) { + if (min_out < 0.0f) { + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; } } - if(max_out > 0.0f) { - if(status_reg != NULL) { + + if (max_out > 0.0f) { + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; } } // mix again but now with thrust boost, scale roll/pitch and also add yaw - for(unsigned i = 0; i < _rotor_count; i++) { + for (unsigned i = 0; i < _rotor_count; i++) { float out = (roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale) * roll_pitch_scale + - yaw * _rotors[i].yaw_scale + + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + thrust + boost; out *= _rotors[i].out_scale; // scale yaw if it violates limits. inform about yaw limit reached - if(out < 0.0f) { + if (out < 0.0f) { yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale; - if(status_reg != NULL) { + roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; + + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; } - } - else if(out > 1.0f) { + + } else if (out > 1.0f) { // allow to reduce thrust to get some yaw response float thrust_reduction = fminf(0.15f, out - 1.0f); thrust -= thrust_reduction; yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale; - if(status_reg != NULL) { + roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale; + + if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; } } } - /* last mix, add yaw and scale outputs to range idle_speed...1 */ + /* add yaw and scale outputs to range idle_speed...1 */ for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = (roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale) * roll_pitch_scale + - yaw * _rotors[i].yaw_scale + - thrust + boost; + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust + boost; outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); } diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index e48bda6918..5c2edef61b 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -67,8 +67,9 @@ SimpleMixer::SimpleMixer(ControlCallback control_cb, SimpleMixer::~SimpleMixer() { - if (_info != nullptr) + if (_info != nullptr) { free(_info); + } } int @@ -77,8 +78,9 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler int ret; int s[5]; int n = -1; - + buf = findtag(buf, buflen, 'O'); + if ((buf == nullptr) || (buflen < 12)) { debug("output parser failed finding tag, ret: '%s'", buf); return -1; @@ -91,6 +93,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); return -1; @@ -106,12 +109,14 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler } int -SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, + uint8_t &control_index) { unsigned u[2]; int s[5]; buf = findtag(buf, buflen, 'S'); + if ((buf == nullptr) || (buflen < 16)) { debug("control parser failed finding tag, ret: '%s'", buf); return -1; @@ -124,6 +129,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); return -1; @@ -156,6 +162,7 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c } buf = skipline(buf, buflen); + if (buf == nullptr) { debug("no line ending, line is incomplete"); goto out; @@ -198,14 +205,16 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c out: - if (mixinfo != nullptr) + if (mixinfo != nullptr) { free(mixinfo); + } return sm; } SimpleMixer * -SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max) +SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, + uint16_t mid, uint16_t max) { SimpleMixer *sm = nullptr; mixer_simple_s *mixinfo = nullptr; @@ -258,8 +267,9 @@ SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, u out: - if (mixinfo != nullptr) + if (mixinfo != nullptr) { free(mixinfo); + } return sm; } @@ -269,11 +279,13 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { float sum = 0.0f; - if (_info == nullptr) + if (_info == nullptr) { return 0; + } - if (space < 1) + if (space < 1) { return 0; + } for (unsigned i = 0; i < _info->control_count; i++) { float input; @@ -293,8 +305,9 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) void SimpleMixer::groups_required(uint32_t &groups) { - for (unsigned i = 0; i < _info->control_count; i++) + for (unsigned i = 0; i < _info->control_count; i++) { groups |= 1 << _info->controls[i].control_group; + } } int @@ -305,14 +318,16 @@ SimpleMixer::check() /* sanity that presumes that a mixer includes a control no more than once */ /* max of 32 groups due to groups_required API */ - if (_info->control_count > 32) + if (_info->control_count > 32) { return -2; + } /* validate the output scaler */ ret = scale_check(_info->output_scaler); - if (ret != 0) + if (ret != 0) { return ret; + } /* validate input scalers */ for (unsigned i = 0; i < _info->control_count; i++) { @@ -328,8 +343,9 @@ SimpleMixer::check() /* validate the scaler */ ret = scale_check(_info->controls[i].scaler); - if (ret != 0) + if (ret != 0) { return (10 * i + ret); + } } return 0; From 6fe717b17a75412c3e5e18c3e678c9cc3285cab5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jul 2015 12:05:45 +0200 Subject: [PATCH 308/493] Default MAVLink component ID to 1, since that is the more common assumption in the field --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index b1d369cbf4..df2a5786ad 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -60,7 +60,7 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); * @min 1 * @max 250 */ -PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +PARAM_DEFINE_INT32(MAV_COMP_ID, 1); /** * MAVLink Radio ID From 8bb9707f3fc5a87213bb567a61416d9d90e6b239 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:51:38 +0200 Subject: [PATCH 309/493] FMU: Allow to pre-arm the non-throttle channels with the safety switch --- src/drivers/px4fmu/fmu.cpp | 60 +++++++++++++++++++++++--------------- 1 file changed, 36 insertions(+), 24 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 2047046b9b..b1766f7390 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -90,6 +90,7 @@ */ #define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev { @@ -136,7 +137,6 @@ private: int _armed_sub; int _param_sub; orb_advert_t _outputs_pub; - actuator_armed_s _armed; unsigned _num_outputs; int _class_instance; @@ -156,6 +156,7 @@ private: unsigned _poll_fds_num; static pwm_limit_t _pwm_limit; + static actuator_armed_s _armed; uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; @@ -164,6 +165,8 @@ private: unsigned _num_failsafe_set; unsigned _num_disarmed_set; + static bool arm_nothrottle() { return (_armed.ready_to_arm && !_armed.armed); } + static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -240,8 +243,9 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { #endif }; -const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); -pwm_limit_t PX4FMU::_pwm_limit; +const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +pwm_limit_t PX4FMU::_pwm_limit; +actuator_armed_s PX4FMU::_armed = {}; namespace { @@ -261,7 +265,6 @@ PX4FMU::PX4FMU() : _armed_sub(-1), _param_sub(-1), _outputs_pub(-1), - _armed{}, _num_outputs(0), _class_instance(0), _task_should_exit(false), @@ -695,24 +698,17 @@ PX4FMU::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); - /* iterate actuators */ + /* disable unused ports by setting their output to NaN */ for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN and INF */ - if ((i >= outputs.noutputs) || - !isfinite(outputs.output[i])) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; + if (i >= outputs.noutputs) { + outputs.output[i] = NAN_VALUE; } } uint16_t pwm_limited[num_outputs]; - /* the PWM limit call takes care of out of band errors and constrains */ - pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + /* the PWM limit call takes care of out of band errors, NaN and constrains */ + pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output to the servos */ for (unsigned i = 0; i < num_outputs; i++) { @@ -737,13 +733,14 @@ PX4FMU::task_main() orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = _armed.armed && !_armed.lockdown; + bool set_armed = (_armed.armed || _armed.ready_to_arm) && !_armed.lockdown; - if (_servo_armed != set_armed) + if (_servo_armed != set_armed) { _servo_armed = set_armed; + } /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + bool pwm_on = (set_armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; @@ -828,6 +825,13 @@ PX4FMU::control_callback(uintptr_t handle, input = controls[control_group].control[control_index]; + /* limit control input */ + if (input > 1.0f) { + input = 1.0f; + } else if (input < -1.0f) { + input = -1.0f; + } + /* motor spinup phase - lock throttle to zero */ if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && @@ -839,6 +843,15 @@ PX4FMU::control_callback(uintptr_t handle, } } + /* throttle not arming - mark throttle input as invalid */ + if (arm_nothrottle()) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* set the throttle to an invalid value */ + input = NAN_VALUE; + } + } + return 0; } @@ -847,14 +860,12 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - // XXX disabled, confusing users - //debug("ioctl 0x%04x 0x%08x", cmd, arg); - /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); - if (ret != -ENOTTY) + if (ret != -ENOTTY) { return ret; + } /* if we are in valid PWM mode, try it as a PWM ioctl as well */ switch (_mode) { @@ -873,8 +884,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) } /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); + } return ret; } From 0ca6f46ef497419d50cae2cc14be061aafe8ed73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:51:59 +0200 Subject: [PATCH 310/493] IO: Allow to pre-arm the non-throttle channels with the safety switch --- src/modules/px4iofirmware/mixer.cpp | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2cfdf491b..050750d080 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -62,6 +62,7 @@ extern "C" { * Maximum interval in us before FMU signal is considered lost */ #define FMU_INPUT_DROP_LIMIT_US 500000 +#define NAN_VALUE (0.0f/0.0f) /* current servo arm/disarm state */ static bool mixer_servos_armed = false; @@ -243,12 +244,12 @@ mixer_tick(void) in_mixer = false; /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = 0; - outputs[i] = 0; + outputs[i] = 0.0f; } /* store normalized outputs */ @@ -369,7 +370,14 @@ mixer_callback(uintptr_t handle, } } - /* motor spinup phase or only safety off, but not armed - lock throttle to zero */ + /* limit output */ + if (control > 1.0f) { + control = 1.0f; + } else if (control < -1.0f) { + control = -1.0f; + } + + /* motor spinup phase - lock throttle to zero */ if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_THROTTLE) { @@ -380,11 +388,13 @@ mixer_callback(uintptr_t handle, } } - /* limit output */ - if (control > 1.0f) { - control = 1.0f; - } else if (control < -1.0f) { - control = -1.0f; + /* only safety off, but not armed - set throttle as invalid */ + if (should_arm_nothrottle && !should_arm) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* mark the throttle as invalid */ + control = NAN_VALUE; + } } return 0; From 433c9bf42d55165c9e45616514cda2765217af63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:52:15 +0200 Subject: [PATCH 311/493] PWM limit lib: Support pre-arming --- src/modules/systemlib/pwm_limit/pwm_limit.c | 28 ++++++++++++++++++--- src/modules/systemlib/pwm_limit/pwm_limit.h | 2 +- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 2f72d347c6..09965b96b9 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -54,7 +54,7 @@ void pwm_limit_init(pwm_limit_t *limit) return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, +void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { @@ -99,6 +99,16 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ break; } + /* if the system is pre-armed, the limit state is temporarily on, + * as some outputs are valid and the non-valid outputs have been + * set to NaN. This is not stored in the state machine though, + * as the throttle channels need to go through the ramp at + * regular arming time. + */ + if (pre_armed) { + limit->state = PWM_LIMIT_STATE_ON; + } + unsigned progress; /* then set effective_pwm based on state */ @@ -120,6 +130,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ } for (unsigned i=0; i Date: Sat, 4 Jul 2015 11:35:11 +0200 Subject: [PATCH 312/493] Mixer test: Add routine to test pre-arming --- src/systemcmds/tests/test_mixer.cpp | 64 +++++++++++++++++++++++------ 1 file changed, 52 insertions(+), 12 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index acde4a1a50..20b77f1e27 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -56,6 +56,8 @@ #include #include +#include + #include "tests.h" static int mixer_callback(uintptr_t handle, @@ -65,6 +67,9 @@ static int mixer_callback(uintptr_t handle, const unsigned output_max = 8; static float actuator_controls[output_max]; +static bool should_prearm = false; + +#define NAN_VALUE 0.0f/0.0f int test_mixer(int argc, char *argv[]) { @@ -72,7 +77,7 @@ int test_mixer(int argc, char *argv[]) * PWM limit structure */ pwm_limit_t pwm_limit; - static bool should_arm = false; + bool should_arm = false; uint16_t r_page_servo_disarmed[output_max]; uint16_t r_page_servo_control_min[output_max]; uint16_t r_page_servo_control_max[output_max]; @@ -184,7 +189,6 @@ int test_mixer(int argc, char *argv[]) const int jmax = 5; pwm_limit_init(&pwm_limit); - should_arm = true; /* run through arming phase */ for (unsigned i = 0; i < output_max; i++) { @@ -194,6 +198,35 @@ int test_mixer(int argc, char *argv[]) r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } + warnx("PRE-ARM TEST: DISABLING SAFETY"); + /* mix */ + should_prearm = true; + mixed = mixer_group.mix(&outputs[0], output_max, NULL); + + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (unsigned i = 0; i < mixed; i++) { + + warnx("pre-arm:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + + if (i != actuator_controls_s::INDEX_THROTTLE) { + if (r_page_servos[i] < r_page_servo_control_min[i]) { + warnx("active servo < min"); + return 1; + } + } else { + if (r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("throttle output != 0 (this check assumed the IO pass mixer!)"); + return 1; + } + } + } + + should_arm = true; + should_prearm = false; + warnx("ARMING TEST: STARTING RAMP"); unsigned sleep_quantum_us = 10000; @@ -205,11 +238,14 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { + + warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && r_page_servos[i] != r_page_servo_disarmed[i]) { @@ -222,8 +258,6 @@ int test_mixer(int argc, char *argv[]) warnx("ramp servo value mismatch"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -251,7 +285,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); @@ -278,18 +312,19 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { + + warnx("disarmed:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { warnx("disarmed servo value mismatch"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -314,7 +349,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); @@ -324,6 +359,8 @@ int test_mixer(int argc, char *argv[]) /* check ramp */ + warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || r_page_servos[i] > servo_predicted[i])) { @@ -338,8 +375,6 @@ int test_mixer(int argc, char *argv[]) warnx("mixer violated predicted value"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -397,5 +432,10 @@ mixer_callback(uintptr_t handle, control = actuator_controls[control_index]; + if (should_prearm && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + control = NAN_VALUE; + } + return 0; } From ef4946f81bb0ab897e4b7ade94d39cc8423fc632 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 11:38:25 +0200 Subject: [PATCH 313/493] PWM limit: Avoid writing back into state struct --- src/modules/systemlib/pwm_limit/pwm_limit.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 09965b96b9..8f2cec6fc4 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -105,14 +105,17 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c * as the throttle channels need to go through the ramp at * regular arming time. */ + + unsigned local_limit_state = limit->state; + if (pre_armed) { - limit->state = PWM_LIMIT_STATE_ON; + local_limit_state = PWM_LIMIT_STATE_ON; } unsigned progress; /* then set effective_pwm based on state */ - switch (limit->state) { + switch (local_limit_state) { case PWM_LIMIT_STATE_OFF: case PWM_LIMIT_STATE_INIT: for (unsigned i=0; i Date: Sat, 4 Jul 2015 11:47:55 +0200 Subject: [PATCH 314/493] Tests: Reset mixer inputs --- src/systemcmds/tests/test_mixer.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 20b77f1e27..3b2f42b21d 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -227,6 +227,11 @@ int test_mixer(int argc, char *argv[]) should_arm = true; should_prearm = false; + /* simulate another orb_copy() from actuator controls */ + for (unsigned i = 0; i < output_max; i++) { + actuator_controls[i] = 0.1f; + } + warnx("ARMING TEST: STARTING RAMP"); unsigned sleep_quantum_us = 10000; @@ -249,7 +254,7 @@ int test_mixer(int argc, char *argv[]) /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && r_page_servos[i] != r_page_servo_disarmed[i]) { - warnx("disarmed servo value mismatch"); + warnx("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]); return 1; } From 7b14a0258e8edefbc5b5eda1a86df925fa0d039d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 09:50:07 +0200 Subject: [PATCH 315/493] pwm limit: Fix author list --- src/modules/systemlib/pwm_limit/pwm_limit.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 8f2cec6fc4..cf71d7e335 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -37,6 +37,7 @@ * Library for PWM output limiting * * @author Julian Oes + * @author Lorenz Meier */ #include "pwm_limit.h" From 87b801034fdc897095bcdc959ae31225f3ed805b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 09:50:44 +0200 Subject: [PATCH 316/493] IO firmware: Fix condition for output enable to also allow no throttle arming to enable outputs --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 050750d080..1fa327613e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -281,7 +281,7 @@ mixer_tick(void) isr_debug(5, "> PWM disabled"); } - if (mixer_servos_armed && should_arm) { + if (mixer_servos_armed && (should_arm || should_arm_nothrottle)) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); From 1795962328ffdaed093495409d09ad4bf374bf8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 10:11:54 +0200 Subject: [PATCH 317/493] Default Skywalker mixer to wing wing gains --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 22 +++++++++----------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 3d464a4ae9..4950c3183f 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -14,18 +14,16 @@ then param set FW_AIRSPD_MAX 40 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.3 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.03 - param set FW_P_ROLLFF 1 - param set FW_RR_FF 0.3 - param set FW_RR_I 0 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.03 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_RR_FF 0.6 + param set FW_RR_P 0.04 fi set MIXER X5 From c05c5bfceb70c6947d9f35c234d68187647af91b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 10:12:23 +0200 Subject: [PATCH 318/493] Multicopters: Load gimbal mixer by default --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index a5c326ebc6..6506ed8c3e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -20,3 +20,11 @@ set PWM_RATE 400 set PWM_DISARMED 900 set PWM_MIN 1075 set PWM_MAX 2000 + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 From abfb0bbd38d6992ffbfbd6668a87cb1b6912adf7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 8 Jul 2015 23:30:39 +0200 Subject: [PATCH 319/493] POSIX: Silence HRT red herring --- src/platforms/posix/px4_layer/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index f51802d98b..2fbe8cd70a 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -351,7 +351,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte sq_rem(&entry->link, &callout_queue); } -#if 1 +#if 0 // Use this to debug busy CPU that keeps rescheduling with 0 period time if (interval < HRT_INTERVAL_MIN) { From df4b07937e0ea66b3f43d7de784ad09c7c2806f3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jul 2015 00:01:20 +0200 Subject: [PATCH 320/493] baro sim: Fix code style --- src/platforms/posix/drivers/barosim/baro.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 8958fedfe4..bcfbe965d8 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -279,7 +279,7 @@ BAROSIM::init() &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == nullptr) { - PX4_ERR("failed to create sensor_baro publication"); + PX4_ERR("failed to create sensor_baro publication"); } /* this do..while is goto without goto */ @@ -664,8 +664,7 @@ BAROSIM::collect() if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - } - else { + } else { PX4_WARN("BAROSIM::collect _baro_topic not initialized"); } } From fc3a85311d377768f8ae2a2761c23c346115e824 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jul 2015 00:01:34 +0200 Subject: [PATCH 321/493] POSIX: Run main apps delayed --- src/platforms/posix/main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index bc531af44d..5d393e1312 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -69,10 +69,10 @@ static void run_cmd(const vector &appargs) { arg[i] = (char *)0; cout << "Running: " << command << "\n"; apps[command](i,(char **)arg); + usleep(20000); cout << "Returning: " << command << "\n"; - } - else - { + + } else { cout << "Invalid command: " << command << endl; list_builtins(); } From 16cb971d6306884e3d238d24e32bf2ca5afcafee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jul 2015 00:48:53 +0200 Subject: [PATCH 322/493] POSIX: Increase app start spacing --- src/platforms/posix/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 5d393e1312..f4e398d186 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -69,7 +69,7 @@ static void run_cmd(const vector &appargs) { arg[i] = (char *)0; cout << "Running: " << command << "\n"; apps[command](i,(char **)arg); - usleep(20000); + usleep(40000); cout << "Returning: " << command << "\n"; } else { From 44eff3681955ee03c9611fd17a218edf32b3ec67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jul 2015 00:49:40 +0200 Subject: [PATCH 323/493] SITL: Run more streams at higher rates --- posix-configs/SITL/init/rcS | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 2af14522d2..c303335c3e 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -43,8 +43,11 @@ 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 60000 -mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 -mavlink stream -r 50 -s ATTITUDE -u 14556 -mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 +mavlink start -u 14556 -r 2000000 +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 From b1b555ceb6f8121cfa87e6dbed1274a232a45006 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jul 2015 00:50:00 +0200 Subject: [PATCH 324/493] MAVLink app: Increase max data rate --- 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 c79b923c5e..faba108e0a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -95,7 +95,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 1000000 ///< max data rate in bytes/s +#define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate #define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. From 3fa7006576a0aa2b013b56ca7e2c7f1d83f32cc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jul 2015 15:51:44 +0200 Subject: [PATCH 325/493] FW configs: Enable pass mixer by default --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index b718f421f5..f1b92112a3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -23,3 +23,11 @@ then param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0002 fi + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 From 396db730a6e6fdd72fc7a1a3dae1d8591b0d568f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jul 2015 23:58:11 +0200 Subject: [PATCH 326/493] FMUv1: Enable PX4 FLOW driver --- makefiles/config_px4fmu-v1_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 2e4829958e..26ee983ced 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -37,6 +37,7 @@ MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors +MODULES += drivers/px4flow # # System commands From 8b886e857a4fde973d7aa4fe8778a4775781b547 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 9 Jul 2015 21:11:11 -0700 Subject: [PATCH 327/493] Updated HIL documentation Signed-off-by: Mark Charlebois --- Documentation/px4_hil/SITL_Diagram.png | Bin 0 -> 80757 bytes Documentation/px4_hil/SITL_Diagram_QGC.png | Bin 0 -> 75957 bytes Documentation/px4_hil/UserGuide.md | 98 + Documentation/px4_hil/docs/readme.txt | 0 Documentation/px4_hil/px4_hil.doxyfile | 2403 ++++++++++++++++++++ 5 files changed, 2501 insertions(+) create mode 100644 Documentation/px4_hil/SITL_Diagram.png create mode 100644 Documentation/px4_hil/SITL_Diagram_QGC.png create mode 100644 Documentation/px4_hil/UserGuide.md create mode 100644 Documentation/px4_hil/docs/readme.txt create mode 100644 Documentation/px4_hil/px4_hil.doxyfile diff --git a/Documentation/px4_hil/SITL_Diagram.png b/Documentation/px4_hil/SITL_Diagram.png new file mode 100644 index 0000000000000000000000000000000000000000..bbd2dbcceee688c356722ca5a86930689fbf0da6 GIT binary patch literal 80757 zcmeFZWmp^B(l!bdO3_f<8mvf*7K)YN6nUUPahKvyq(})K8VFE|w0LoMFK&SpDHL}n zxD*KP^jx8Pzu$h|Z@>F|=f}CupYtyyOJ>cQS!>ok_YkJ8sz8KKi;sbUL8PQ8r-6ZS z4}gJj&*;H@3=9mKu9zqc3`PtkIcY7h$**}FcZ$gc925~I7L($;2YWp2^SLA$_2da7 zGJ(vFPaI8^4Rc~CKT>>bYN}Cu=ZUwsw?}d~@jEAGVe@B8ga28{Nolw8K&t4GSj<^} zYxf%*TwGizE)ItjCcZiZi#!7Z{qa$t9wES2{_kJ1U=af3WA$MFY;@Ni196W5>tA}} ztHYx)5KtyI%B;JQ{`nI%@SN~}jgS5ohD{A5d{#;Fx2fSMxXph_ao3mcF&t$U8W#L_ zSzx#l_x>U4pBW%bm@r&f6)w5|ni1WB4}_xmv!Z{>L5Rl(qJHy+>7Rf985S8h_%~@e z%oHKWb`mmz*MAd+5a4_N?{dgu5dvU|dZB;Qi5YlF;BV^0rzSN6^L_s=^EZ((5cksm zORl?N|9{o}f1|pkF(i{|HP72WR^g%<2=`i*c_kli4iEQFjI~Cui2vqqMC`4V=r7_M#)tfGFVcfxudBN7hfIF!4o9 zAjtLuM>s9o5h}DG$aW1;|H!+ZT%=~;iqHrKw9_n+V-W&WhY3rt(N;0|&648tG7QKZM+pl7zn7gs2Bg8 z!4Hv{frrSJ_wS@B$3Q@jYKa`~*2RrO4Vfs?ryI2p&lbs~ z%=9}kwdbdrsfe$X-v@Dkbw{NLdSUDoW?+{Qn0XD_^z}3$$o8*aK~#53rZ5Ae=4v-m zH6d8Q$8gk=bVR=)dR{YGOc*ZR^5NLsye}chc71~nCuj#>n!zFjl+F8>h@%Jin^(GH zQv=UK(V9VFV8U<#ww4Rb=tg1>_&`&bNeOq|Z82fE3UwIeYUn){$EF6(Wk2$wLQj)L zVFq5hCv)eh6C`Hf3K;XCG`iIfv?VkL>WQN@%fk#uEin^y+G{|t$V0<05V9)A=lXwi zj-H?E|Bd;NJ7f=j(e$@MTWolD{MS2>_li{7fRAMR>^<;OgiX$lxc;0%1FUe=lF@Ga z%$?nZK}^7U({16?lSI@xzYhmdWSExA@o9wc)4>&_&*QBS7@kw?SaCk{3TMrQ@h%yx+(FFscgaL zLDD{7j)xziVJ&8U+2TYgAp*+tkN~Y`mnWDo+yr|bop`h!tgzODLUM0jSiqYFMZn0S z#;Gjc7g=Yqto802t(W`S1gmo69~y=NCh+Hb`9Pbbm3P-s0;w6;rEvX?7203**vwSH ziSxzWY|c}`Ckm>C;jeZU@Uti12G%|?DswMHY4%|}rxDVx<0k|(%H3@}JQtf9XjIgo zcV~(5c!^}d%dz4QGD$alb_mtC#Gr8JWqC?iJg#TF% z#iS{!dxrOy))DOIi*wqj!LPu3UniwqT=+~{{qHgCgY{}RIM_-SGYx5AxW>$scTT2( zJ{!i3%Cx6F_ZN^O2Nw0d%p{)W#+`jOgD4)32M1(qa#hc_qMw-hTRu^LU}9oY=3()y zV7Hc);N9L3CV80#QSeip*EZgVOA?e()G)nDi|l?+$SoO`uAX^#8OJVESS~IrV;=;WwGAvoE5**5e~C z9)*2jm5rk;bu_`%eE=voX@-B4lt*@s=0DgRp%{^OwM8( zbw!=oO`etB9Sq=`LmW~zrH{q2`5xqR{UAUsEd#b|Ibcdc zk^H>wifEzH{b9hXTe3s$hRHVEz6{tRI-W$3Fj=>(z`cr{C@`niKB2 zPf=%mp9NI(X|UYfcdX|`0w~oOBxK%}Pob1ju(#Y+b?y>Bbp!3V2KL6?5#{TScF=%{A)>CP@lsUm@0 z2dT5#7i@lQAJnqsw%wdfS~|k!#ay<)jKIaFtNj#W)0}}=8ZYI9m-ubR<^#@7;0NAU zl=-K-U7gtqsHIcxW&&^A^zUKOJch*N9HtLjl@$@gxvDp2lRR8Xl{x6&{g|cN<1i3IfXs*>)sXr+?d%R9E=&Rw;#nkfvAs!qc zG~mR5Lw&CeNZR6aJhr$K9I`wuP=q(4yC(s;p6$uxo~Jd|hvBk(;gUt)F&^k+EWgw! zE6DIW`D_30PjL~u8Z73OU+E-tm z$>@OacfEwaTQ(g{YpHDjzhsfU<1TNnC@CsP*rSZ5d`0L<)m zM^X^zYv!xVCIZ{QG!_)F5NfhR?7JB1F9@=|@GIhkv)cAMAIQ%|QhxD1E)I_QbOF*{ zM00_EO}(V+Dw8*z|Ju5{s?)wR%hGN%bFVucmc>mcj>Vty(!|@UK6b0__7JPpds)OI z&DzwL$eDrRq=Br-^UdHk#b;tegBRW+!1;#kb;sBuLxwH4Rs2z@nQ)h)Pf6-69#?$4 zv-xNHh=H~6@j*+yhzU(|RdBRYJHM#jUbcO4ZRkE3|FtsgwkJA6mTt1xT>3=vhU+-l zZ&v`1lvTqG^SXHa`L#OE-c+9V(=6H!D^>7RgD;-y{vuswM9G1#t?*;Fr|U$I9wZew z<|Op-1tn8Hrv*M<=iZv6Y2%uQ?6MDb>p_uBT|ppBba0c8zHy7zU2b}Ro+AUmRL7KY z$pN9CykuH`2%pvaE{g~aq!zaJJJ02t290~K(*3@W#Vty()tA>Iz*AO;QeW(`;ng6w zlC&j9R=EY-;+3aKDwGm46q>)hL^7@VZdGllBjLxnvD zHd9Uz8 z$Lc4kKBxa)^rJeA<${vd+|#1r1}P>+tD6s0T8_DZT748wT!Z$lJ|Ed4nVhcA6~BO9 z25BmnNB_KvrYKpB7G>aHRTe8b@l7-V?=Kd=^X(Q|<~crRX7B8tp3%X7Ci zDJ$Tg@mpsGg8UwCr4h?}vpohW_VAcKUG?4GH#@^TNuU2p?NI&3%QZu;^|$x!DRx>% zE13;pjX85Vc2BCP6v4Z zN{5ik_r@@;AJ$f%AF-eP`o5>lsWljS99@vW22K8Mck%_PZ7_rvK!hj6|C`D9G^Jpn zG4-;B66i|78?1qO^7AnLWb`aLS0{UWYiRbdYExoREB$R}G@J6ogUPzCqlfxpmOZId z+6IYK@jpA>K|9uia1Bdp$;mp(BJOoyM?53YG5ggKy}<28j7O>+P9=ZyF&7c^&~2do zE|3QqLy+yG=@^`gZKu1fDpTBAPb_h*+yF@&emB386we+E4dnI<_h@I4|22;1r8LnY zU8A$hTry{Q=`-qL{um_pzBSuW7zw|uu_twlsC~rJ(kXy@T6jTIW$Ae1B(6=9jg%H^ zdnlGvVBDms2pR5>ZLOkEFOlbFVE}Lo>^9wQFB18jYA3G;OJ>p4K1LMh2rIHh^*vR8 z-CtADb!2Y9+b`1io{ykCf*_d;XjzjhEWJ-cPJTl?qQGl9NpV%afo!`Gsoj+*kYLY* z_b`dyNBRIyfs&4^+2HG6qN*m%AJ_*?m&S67w*^dV?LG>56wzLS{(wPJdT8+Jp$QBHkuWX$zAffOd<&Wdbb03;;~O&MsXm z_Sq88-b4AgbwCC8CzIAeMon%2x>Nc$E)~}Ij*Q4NAl+z`!6;_-6y|Is5>lhdUKpb)!H>eld5eT;HCUcU^cfI4hrN+4^*}A^(1H*MxmBCt+v%U0M zbpu-J#b7@ez2c(DXs7yOm9AnL7BNvX}F82IGZ*;umSAp7IWeFj%jo2 zI(X}?T=0pfQx0(@t(cFE8TTg90WE(_j5Jg+)|uMpD5j89*Z$IMQoCYR&Wd;OJ6w)q zqP4*Xz@Gm6gNKonqCrHGVuGH?{wmGNU(eLDjoBY7J$&a{YNRFP8skT3`Ngre18_oWroQ@%Woj}#GRu9bO0dW`ZybqJo$JlMRXLx`uw>9bU_)>(PoM&soV7hsG z1(UsC4uoeu8kB09IM|ClY-M5Dagxf1*=-AsUQ^g0dX%G6Ry5s8Hewkm5+Lob2!mk8IGF{8#z@6fvj_A{+r@gNZRjk4CA6$*E_wAMo+<_|61b zSPUy*g?4lUacntro%qFLo5?dwqZXVpzn-!l6jD~xn{AX(4a055d7+27goYZ^Hg=Vuz z16A!um2V1}MBXiTbslv`jSHW;Dq<82=kUc#uXDqls3DMO-@-19hKP5c^JerQe|N?V0P5&BQDFbz=ew(rz*rZ!t^gD@mfjz z#Z=a!`M>r_*quOHcGCv0iYAENIuOaai3&)+!lk$wEHCaJM}41?-ybi^r0x3(Tl*L${8YHezgS_l5%Psa%~%^MBjC zRRt$5WDBl4%|?ymMzWM8P{sRBKY0%7i7TS&4L*%i&Rh$%YovO=j$#xC2ukb%^n>jt zV;?r2b^HOJG5A2JClyJMX1U%xi-br9ZRL->UH1HAimlRWNle)@nA5xpV>@Mi6?qo3 zLMxXI)!^In&$M4z#Gdx8yN$3slyKQ>`K3@3>#%fMDpG5~Y0?J9O#dD>e*56iv2Ec3 zd#`_IQl!$MsZnj~$!8O?=_@AVT6%kndemZLG8u67nly1W`I_&UlT_?yYj^Fn737j&8`N z5#S{rsyx3gDeq6S@D!~SMVjlk4Qxs};9!-{eR>We@BI~m+3&CdWg>I;N)ZJquF?4) zWE%wA_5SvDyU>}Ru)ITp{sGmK1=ftRA-gWzIE>3NT`9Tu#)OhL2HQK~*&GHfl!pn$ zOJg1$u-+sxV4jC_@XID*i;IQq&#(~6wU1fenRAZ>SDRTkF2kZlT4QNU&QJ`+Y=QfzJ!UGvDNTipuEs zSy})zzWQ;u!~aO3Jj4;=O$-n3j?6f}@`$d}($9Z9!{_j#EKMS;is8A;Df7$Js>lJI zCx0Nb1HbB-o&=^>oA%1iA3kXD2GIxVIxRId6dxS+8_v(VpQ18$bsqUsIn2{p+$j$D z6rP3)a0D+1;bf6UQ>5s8qKilvOk>XI%UAsRG)~hzb)gL)mcT0yoAY{|`;s%iC|N+a zD+R@P_K-mR3UqGM4_OT(Ei#PB)d!@teK5}FWu98j^PqBJ-P#f?63#{ zX%oxMruft((RhPZ3{VxPg!#&qt8?n}*xm5CR#%z;wWNYmVy`v}sv3L>fCbh%4A;VD zoOdnq4z1try0;NAiED<3Mp>QM;HV{QqTbm|G(4RIX+UPW5Bvn5ZVN^lHI}Ke%O-wJ z;QT0u*IapWE-IKg)6VXRG2 zj7jBy+v^-0lU9GI!x;mUGweBajkmxA@I|2^!~X_um2jb-5q;P{@c=9@o_7QrdLCRA z^>4r7?xT{8;SW9-W^fyC4vExOE=~(w&`j9;kC-QvtN#V~z<^#PK->7&%v*nofCn|Q2u^rB1Y)3-{b#`??sy{+c?d2O$izfH@?Ap&Z9~FNPe(mo_%nvb5@(D(3_$0m0rd_ zuss*%z@;U#Z+04wY*g>Rx$SlANfb?fWq)=(E#Rx3EwbvZJh}U_BdArVfmrP?OhYv6 zE)%zLmo2bF!=gnvu35VCy!&wjs`0-+R7_Q_Tb%)AZ_=J-j?BPT)0LW@s}%JAG9juun{S$tH84u{PXb zXlgl_G>M9_Tm>1l@B`5J-m2GP9C(q z9{_1bum=?mR!Ey#>>W!ooz?o@_&-+tqIGk7S<_dbTfT;56X@Y!hX z>xHAl9|YPhoC!UV=XkyqyGywoeNN{-IO{5OQAZo9$mvmb#WZG$qVb}rvUI;(3A*Wn z$&WA3*N>uxj}ejQ%d|MHTY3ljR$t&^u z{ewfQW8F5+B;JTB0dan-AF!7W@6Ctj?5MB(Ox-VY01|uCgDzs#j5ze%aMaQ!<4l^Lie6#hGSqFfv6EXbJ;yLKF|FN zvn}=qQBBZiKj627<2S5*!L90xjqiv86#zv<>umid!5?0JT)Cds+#JdhN|W&VboM1z z>NLg6W#jaf5h&ok1RmL&<&wCbB(XN9mQQKjb11%miN}ODm8Ly??_KyNoFp^1a~~OS zs1M5Ls|{!Gdk({JTU-smil3hfekA99glrY_AE7H8%^4*EBny>DFFH9!bG#$fDg;8# z@kq^fx@w+|HJjEw^S@q;QhYTl9KfIe*hO>*ihUMUey_d0J6l(b`)qIbMbL(ZlayWD0eu`Vg!EMmTDTuoV8{E}>EhX+X9cjK4cYnf>tl zN(Sr*KuB7`H}phop3Zn)5YBu1MF3NiExIR&3Jj{tyfY)Hcr8~DIMwVc8w{;-H}VY2 zvw$5P@n>FhF^k7qR9KiqU>hh5CA~GYVG?;aYCi#?o2ll^-nGU@Pzr1 z_8`K=2-)oCeRfpI0H>)I2DzVa>J&CT>^_kn&wyk5friAjrh*vFUEOX>4A+M$;j-tXob zy&m-m|I%fW#_FxMQF~pc;W0k@I$wh+md6w)o9$=#u}SOAHCc&%B}ssaafx>_SRGD* zhU_xq^c2vi^D}ZYa41v8k8TT6;QiZjK>W}2NS^sNsh!C5BC6Lk6IIrIuTKu`yyu$V z5!sDgZs}aF^-;kFEmXlnSW;ml6sUeR#5iA06-5#9Omai+0pK^?xtRa;y1P2r^AOA(q4_G=a-4XFM1AKYx2dIvJ`#pKGd?>?ONzlk{K9Y=57uB141RErqHXg<2IK5%X#cZ>h4pjW8QMOofv()}jiHwPIOcSv(15355du!f*6)(PB8m_slj}Ka@_V!C_0$5>%ICCNPb~^Oe&PX;YI+$i z&j_zQKqD7cK`Jh{8S;21%&q>DPh=+}OQJX)np=CzyZa_;Bt#W@!xFR^Z zQHi%butluWX2Y6ZqJj?hY*(WN^qPp}OUC0sC6nbfD-$u=Ngn9_{fMA%^Hkz9*>5}T z?M0G)MnsE}l45nKnST6~RGsBHIU(ZeQvOwsCs4YK7g=E&#=kv73V$s1DY+J#pFzIk zRdUz(>%#3TY9Z^!&Or=GMNA)jKG2kA^5k7wpAi!l(u3^g(hIC4(9LpK!Ekrbm{i|M zBm;gE1#AqY$c0~0j0eqj2JGtuBKwmk3zpJf-sFGE2S~~(!G>eG43&sJhwbrgfaF8C zl>o9w-V`h?Z;@HKjN%WF2zXmXoTTuHVj$Kii!XSp%?9_<-7z>{ExjOD%PQ;YXk(CMP)wpXw)}&dPe3+I@W)3z zw}g;iT|8$z_ZiO4NmCn7Bh(`pTKB@%!mU^G%~xJ?W-uY3nQbDJu0ohFTpNdn6CvoN zC=oU_ke&{>x;^6?0_&g2_zf}D4qnCaxiRf~U1;Dq&cbA|Hm|;EMB}-k_lA#t4y~v@$%;nIN~)d zq@Y)y1=J&a%}`SoQk@@ZaoKSajZ}42RFm838rN>=QZvo-zNZp3WUg!qkU5%29bet_ zC{8Gn;&UG(@3Gexm#`1?;;b!r{{ zhQQN9?l|ga$6`NIKsww%o+?+v000f^xt{d#UKQ7%#SQ5luh62D1&vaQqA@K*mSs9F zFE6iF;ls+h&2VR2i-}lCt@F>B`mB9g8a_5DFZ{Jy>hn+fATJ%>!^+;~BJIiYlE3|e z@curlzC1V%%YJv#i$ihW@?tV6rps*kS6t>UQ%seWYi$WLt8Dwk?-~_uf~NEGdGP6R z;kKjq)hW?at+KFW;qu}ojh?n3gCbpG$0tUri4%U`_2FUI)WDR2XL=Oqo1qp>jFJSU zG=p)V`2(5=ZIvkTK&B(IzgGJwkU2LqLNE7z(vXZRU*<^qBN*BMIx8FOgZiZd+z4B? zPc*Pch7-kJxOl#+!F9}y?!Pr!Vrj)crF1KTd*bRKUpG0ED}M;D=7lu-_K8~VhfOOB z%hLia1+y;4KO&J(pJ7-?Pw$o^>12_gZjT9o6qxnGa(e9B7QY!X<%wRcZS7aX!{rMB zL1$T=NK#!q5+uN}PzTRJfXZ3DEuIL8f0dv?*2Vf5RN>$bM>J*H8nmCy#$&XKZXN7AqaOUhjvO>9zP`CfEyJMpvh3KHvk1 zycDP)Mcc<)PB_ZU9~`aL6SVDon`m9|&i^u}6sa@`Q|*y?Pc^+jRTvQCYsx zFWfIwxL@|NtYeK3Ft%X!8+d4lKd1#!_NT(y7tge>#Cj@Nf#fa}m@r&jyC)MdSmYT} zgxJ(TE&$*m*7r7~G~Iw@-x3LwZ9{>ZBwA_Q@vByMQcR$I+s98yvTTNiSK!-UG_+$} znQX87%I3T|?e(uNK@2!=nBgcAGI9SqN<{brKG1hAP!ND*W}2GbXcYh%&%wYcVS}U0 z1pi7yGwL8m_)4M&+dzcWEowIKJfsevVfS7)?GBfgK&Tfj6$UYN^=~@fA(Ma#6;F;nuy?Ss=&ZSYpB=1OP{dnqS>DV_s50 zB4<*4>fc}STx;Y)OnX-cWWIafUCp zZKE{|Z+(}2Vi90J$#^(doTUJ+&pkPlpKn;%l)S zCFYuKCOhGbmxumkH3vxYM3X1sy;y_*o%?HB=(n*tJP{%g6!Iq>5Vtuaf-(PfOi<@t z{M}8}j-~nt2&y**@BGmXE+!0DjPZz+JOg5g=KW~99a{exg&q@z>-J6WZr@`Dz0B_Y zgID?X2^?jnY@C*iMed@2c?znxC_YU4)8$`Sq$E$_D0oQVot8m55M;YWa+Nf+ll@(L%K|F>s7zAjo#suVHcM z_ovjqzDDQtA;|Vr!-TUx!wsX!qnKzo!7L2}0rfKBdUZFA6Pl1FAMwL569eZLGaO|` zWo;7lXPVa#WIK&vutydKP6~RkB*PGo?7L}_(HDGd0((9N&YU^~*-k_kSNDgh+J#LG z{3KdQjuz$>num&o{)7)M!9YN}g~Xjxu*fUWJ!C}u|CpJP>C)c~rYu0V`5syULX>9U zif4RkXb#(d(N;Ylnt@#s;#d9t*H`%J@GdMufYAN*Ewt(X^M@rK^mZN!?kw{RGz0Fw ze!xy5;@pgQ75{c7&??z8%fEBPR=w~OVh3^ev&(z*ZSKr8apzLW_#(34C3|J3OP7OM z4k^rJyg%f~?0yXJo4fWE+_7b^2`VkRiMf4tJ8*m_3%O0&yo1XY ze}`&}+sjQehj{M7%l&-XIaTSeo#*5%f26X{OkCxvsnO535cum?y+M5vjkL2fi9AV0>o-7|9JWAjQGm(WcIW>bb*Cu#P zAX{xjQ0L|E)FOUD3(BAQv9^x#)ef|G*1;|t#Iym&c-u90RNPm26n%Ub8`4FD2q=yk zholT9emz=Ivw}~YL$SzFjF@$VfZ)E^m;0G*FKxR#rKP97xr$mx(U*Bs+t;0w^f#S6 z(A=xz^DZcBvvr$WK)t+oc8sjGpBi(-$LAI)Ene= z4nej_ZS>2x9Ky<+d@Q%6gU0URZw zI7Y@&KIWcdeu;aN2-H%6&XNb(Aif*HSrCX_hsmKjeMJsx5l2dX%RZpIM)K%VDkG-< zSN?m2qc-qrcD1244+2oV4{mDf<$a3nu?U@jqu#Z|=a!65CrYEfzWSOOkQ`44cotyd zbst|HE^`-c*CW0K1IW<_3AhBJ0K-lCJ=EaMI=;CWnH)-gK_Cal$OACIPQob}6#_q; zdmoFa(DRebPL#-0evBKoSce7&dY{y z;*805`u`mrBoh0d5vSks zJzy=;ia4|bUtBkAXGRZtd3l+`bL@H&#T8=Ltm?m%lszUZ!W~Qu$HpV$2ceuJu*hBT zF=4nc+8ZUj^^Z6c& zA-^8JuF#?Zcb^d-HBk+wA*U9^?Wr|yms!X54bCpi45 zq2rzmsZb1rY&|=2zRC0aqFU|UGTdLwVT#((I{;|#X6@%zqNEd{#zk&7yikDYO3F5b z>*lF4oL1aFsY!j8p&g304>Za7SLRQLySO0$6Bgp)Sb~G?Rry;9OXm7KlXU?H2N|OF zq8OmA!{M9ZK_?V>`+$LLm-)Q=lVMIEo}@BP1OSp=aRLYlF?I(QM)jsnt7|xSqX> z1BozUxDFhL^k^$fF@hiik?lViUh70*k-I#^gyB-zap>Gd%_`W`Kx*Kd!)sJEy6JOt zC`o|+1d_VzbPwo;rlbE$?1XU#-J-!;o9Ib&z#t1jwu{ev_C&MJ8PQQ84EHt+y={NH zJym(ok}s`BaTF_H;@5{_AfVyT$%~cHo&LKsqbpBv&^rZWyz7J+CF=Yci@XAgfq*`W z_PWccL{gf8&A=|l0zEsLf3|c9xZBf&Ui@9c9&zut+&>lFPWusbS3<;2HADdDthRZ6 zMSPmpQgW3<5CSORM~CO!#pA3l3*h;|*+zIG4kzDx6&7GP{ap3)Pi}(KmI342Q4)i0x<#eg)lah1A12nK2lsoZux)u z9~th-9kRdlDx;s9V}1Wf!Fil@SpD&$D%0NO$LB2R#m@qB3@fQ@8+4 zlXP>w&!GZ&?NVN<8WtJS*Z=^^Lc8#)ezDhWZ|t5IXvv@X|Dgipqp1LwWbmte3>=Wm z|05Rr`vD-Ze&&l4*|PW?hWX z&X@ra?+7p4sqK>@NKou zf6i+mewjEv^Fr{HNV@)I{MO)1hq}d@;M*&tYp!x|y21U79~D3TGaY{@--H642v(wd z|Hf@e-*+~L(s!jB0l5f3UKCGms^^kFT^kG(VzK$pa>(|6Z>65%#}H)&i7Pxazajp=NtSW`3su}h8joZge_h6imzTc9SGKgC{CF?* zKiP;dNbh}IXu9huYUn1R^4w<8{L`;1ydp~8x23b|Ug)i>FNGL}gedEl;|}UKueX)H zp0B0xvLMfZ)ZmQdD?gemdIy=k-VZSWqXxfX)-i~`?yKx=i=f9HFdgPz|AovPrjlG< z%`HwmY+K;?%$u-xQ*rkwQKK>gqmJ1jXKI-Ea!;4+kmgpudGu<}IM_;NF)!*qvzQHm z=idy_1A&r0kt2`13w<7>O@5(6K{WQnlhxT_A7!!sY6@dLf3KKsOA8ZdEB7D>R zF}S2qvw%q@%Cz~@0RmZqM6Pyn+6ph}g1l}&$i}~!>qWCRmz2<{ORc#zxbUy-dhwdT_~5`0rBH=tCFufa1-1X%GJ9Xs33H457}Kgj~u$A`oEAPuiuJT zfHx)IKaF=>2s26=vDo(Xix$aL`?Md!e)pz$f}VC{q*)DJ^9ZR9M(@x{Z;c^r8>pX2 z=wJBxiQkLO(pt;Q=?_UGnO+T9+X_vCM&F_Nyv*~X?TUjzLAcXKWxj>cM?WeZ|8Ne+ zA`V*{g@_*crKo<5)0&al(?*M>|G*Z(e_WoB1g$#!f@5zdMpUwaFFCe zvOAzbK^E)-%urKW@}J29Kf_OJ=W)56DqUH_GmMl1sym?EI)?mb_dCk{W6P5&DKfGe ziFGyk>u~ha{&}zY@}mtc@xhcBs2O~LudL$;Uvm)F;&!#{50%FpmvtZ?IdU0Y7W%^X z#%T~*QhUs{`RK!ktVQX|&9$Azlgsjr_Ql5}!9kg*7J@u>f<;ayP8NOd9OWc>qNj0{ z$1B(9A~o~`d?3`jJMF3kS=Y;LpB2>Fl^)*iRd1N@w@ba9Y9t6FyU6kj2ShWr*7X~7&!HHVYvTvv za2jqjSY9p%CDvG_&-l&o>XX(4?Zi-(6OPCIS6QbE4Z1+b?x{{J7I{SoL>z%Ad09Lb zodfeIVzQ}K5o!^4>wr4@I$uv^5xeHR3iE7lk7gXU*(ObC^rsWw(USRGvUPCrEw}#Y zywE9?=HHb(K}vCc(Ez(tQ$xC=(wA4>zjEZ)`Z3mGWEq!Sp1&(C`(oL#%cdF z6Up>4D~Nimh)30^AsERiye)+LZ;Od4Y9K8gP?gx*o57^*_m?M|wmY-@4mG)3;szFb zwKsZxOCrnOUgkd%Qe_ZOt#96$Mz*F67JwP)E0F z{3dDisG`Kw<~Y3uD^S--o)(C2b&7zj*BrD*<28<3{r9?;yLziy$_fv#h}$0XDey&E zymcH!yn#?r#MqOV*~{9W+g^ziEfkcyFFvjLM9I?v4PrSzZU?(&BNFu+Pptok@uE>K zM{`?xN~Bl!seUGN#oln|RBeaRU0j z`eLSRU`DidR|3N^VN>hLmrSX&l%90G1twyt6I1G-KvISc^IL&y-67dDJ!aLVr642$eslk0e#3CkH*WEUZ86+M|D)S zM+-V+>C2}=gY}{hhM$Ro3OCw0-@mPaIuA8@+WU$Ey7BoyQm_6G$u_fk|65&KN`6(X ze?#5LMXJIV!FTA~l9^nHe7(yKkH2Y+zMxv)Vdqeein;G`w~^p}rp5S35qs9z7;9sH z_usc3@2hevGRFDR->~wbjq{xJY%gncH!o51N@sh-RaB+rnfY50Z(6*Tl}zoO8rkR039xrsUcarBWRIt3FEUyx1RF0Qg0+=Qt=xbJCf`;^SGY-@>uo65S%*Bm+nTAtXdE zN`aiQ`@$x_G7|in&+SEy;%c!s1rX#D$wF0bv2c2CyE;$75)p5SyO`$j)O;SPiDq#= z=iP{^YN%$mA~nUvY0=!}*!5VR;jZ2jw`tTm&ybT|?MtLo@?BmTK1!JKStqr2o)5WV zyY&cP3*nl4GB}^^P?znIOUjYN663um1FJGZ9KZ4z8%fRUo?FLg7zXoBwB6RvVmFn#eU?pJLaA1Z4w#New3YPm+l<+f<~ca8BqknbtIB+t3w&Drv_ zK$codk?rj)`TY8SD-gx*KiRsXP%l;l3hLL|2c}HD+r}ze@F_59wI?6Q))Bw3LoROT z8k(HGPj3x^My?X(%D38aVYt_w2p^&ysgF|ROQK{Chx{!2aod8Jmq-ohLK!+ zFB-jDfDlkD`_?$y#p$@f_#u_^Vs}0nmE2SQJU6lv4RPO2`4bsCRq)WB^X(}#I$=3x z(s`w)(?OA168P7~&kt|yGzzY6+&i$EZ-lX5diZ+{Cs5JQ$TKZ|EE6k{e9PR(c6+q4 z-fK0JCmv0;-Udih=k2cQ_!Ka43_iGwF!*xVWW8^36?P;Nu){Nw4_2G*4tqp*Z@v8W z&XPk2_0340gm3c-avjmV&1yPP`XfyUXQU-TE>*3yw8iyIdeerP06=kM)xB|2Tx7%a z`onoBgD`Pznh37`GMj1FbB-hy0h2Z&nX@p0F+Nh3kg*1b(!-(x>@0;r6Gn^PGp&}i zHt&em(fH-Vwv!@#5kTW)8RTZ8jAJLkd2xB@qPSPygx)H|+^g1GO`nvF@YT*R?GHb{ zv1+4yTZy;CpyHWRaZ`pdt`tL@@VKjc&-V5m<%@nr^AiHh5zueb=vRaP?&;7H`yf1l zs3`k@H^zN7^zW}WvgecRf_;Tl+*{(*-)yK3V6MpnvuX=b>8%bnE7oQ5zZO=_7*DsAh7t z?bFOl%DLm0psJ&*uBZ^x8DEUTs2UwM+N;-^AG(U=yY>l@j}`^a zv-5lpo;^!hD(8JeK@F6Fai=!CqqCTN`*PrlOlxjVHufoD2M(`@%?f)yrVzYpO5o_oaL#ti?VTs60G-0#EKLYBu{K67Ekd`7oMMN(O_ za6iv;(_CY1TKlWwnfSLM7N(NXscCb6c0{vfeS&ZC?ZLTMW?M{pzG5s%e%X8^AE?kQ z&z<_%+!zGDq%Tz%cyOa)DGuP5+bV{Q%bJYI_Y?ZrjY>Q=*WC8p?9lcVu!-9%eP5?qJdwXKk*X7Qg$#e5t(`_07BEV;y-d}baZ{$K?Pai+Oa{P!N z?`n%Q?>nfbZhXF~|4h8q;j-0xE7nFOP0$8J4b1SC@)<8JnbEyHy_Ims%GPmy-LE|> z$8ZU1#mD9ZJnGylQ$ojgwADO^~ zcPs2ENSxA^To(h;{f9C2{m1PRu7V%pl_Eys`QRXYMu&`801^slX*_6n+Q1A$FbtaWz{p2s? z^=x3FIa@V9IAqwNcBvf7&TJ$jQ&8Wl&bwvj+66^2mYgc>u6f@^Lbqo@quEkj?$mQdWwd`iUc?BI zwB)M7#=JN+Y{i)*rWqNj*D_yKO*bG?`1p%&zUNi%&&!es+G1YLBHI{mE7!xG$cXW} zHAjVhLchR@$Dp9} ztrqOQzKSiT7K+1Z6z|!WL9CB!x^B#tJu%IeA(EEVTF~@};1B`fy$s#QNGSrJ!x0|d zeedpOiQRG3w{q(;_;4SSYjk?oYtdaijZNrBiM~mIbMlKow}-mQCl& zb$CjPmo5>RTNf1B-dNg!GBZ|7LuXMT**Mfd`gttKS12PQo|&#UGdKi=u=r-LKnX0O z>P45i6glGnQkp*8Y?K!y7&*So+pFP}SqB6y;uz?hea7o2HGACC!(@?cwZ?d{{(8c9 zl32L|s*)AgfRBvY?126(kM?@#+H>P8>0I;myF;lg0ia5`b_-w2)t?*GgD-6{@s`N# zM?nX)wknoz<#PiTd-*~~Oad>{tZ>hF3;p(<6covLUsD58+8ggNqrZgT6IG*e(@Z4? z(`m+%-z5~ET^%^Jf207CuFPqHttpkDj%d+~mfDZ8Ti5swHMund2$UK3kLV*lkltjB z4LWW6kADgRi9jHcV8uCrArp9&5-Z z<>0~>kkVy!`*T(`KkY(_a>u?~>p%RUaW&dkove0Flp(3RT7)>qWziynsx#I>he8*$Glt}wCt zmK-2y75Qyq7~6pz{5faM>0RI~QP#YNm57zP%aqKjGCjQShAjV!TutW*%r6{p%)FJ%P;DADk-=qjF4Q(qFO z0{yUzcuXdhK5EPRIOpr}ehnZYd=zbDVNkP;ImZq}VW|qNugf|aV|BXhoyL={pjiKG z=YVBoZ&s9SuX0G8kc~JeBi%od?bLTglUm(OH|{pZMLQ>B#9$C^GitB$PVrZt%pJXB z)ASoBZ8N7vL633MiFHD6-I~8W zRF>~_*T2&iZ^158jy4uBf%WJ*RW!b^KkK6Wj%uP-i^pyz>1Dd^fo4t2p3~@u!UP9=ehj24EI-o!xl9S28l> zy&+ImHuW-k=Nm5gBSz`r-GyN|KxIDv8Po6o);lqvyC}uW`jtxfu%m=8!n9eZbhT%h zE_MRiu$LDr+GabCb-7tO89eQn3fn7P?@&IIV0#ISxyR*?Mg}3w8mi40b zMG^e{q4QE?AUc)IKf)eL>>u9IIehF0d?4hBg4iD4+mGs@iM3A~>pZ+O-~}g-rIBp# zz44W*5xkq`V&h5dQpk$7FHLlu@xF!eHqq#$7e>tw1#L_jl8B_eb1*~8LF?>H1X7Wmj2^PL+1)x7s*~%@ z5~^>57rFpbX1Zdi^%pv6#)b(rZqneOYk3qjL*a4HIqA7`Dj8^DM7~`b%}BNY0dy#; z>W12|NrLO=6Xk>*uTjbeNNqJ&f2fz3uK+xbgBOQfJu%Mj@m-}qmGNaHqNVFL6 zhD~7RkXq*o?*G%g;jNVz@y#JZc&q_!^SxE10NJ(OX|&aG#c$zXG7Fh&w)`67 z0vp7BW&A(dU}dWG%=PzAdX!kwE1i%TbGdND^vS&Lo*zjT&(h{+`CrJb4=BVS`KySwVe=JH%(X0MYBY}$jHJ#l@;rY*&9`RTT>Xvt7v3r3N zmScV&A$LFa5l}Ao8cQd6?~2goE^FP_3CTs>6B5vg{N%r@$F2@=j?CHg zH#rKEd3A7AU#K3Cq_6S)F~_x;5ug8tvj;6PXd(djUsm@O3Jsn)WYgncZu^ZGuz2KM zuwv#Ck;y85LySa!s7b0X4tyn@;ndWP+5}nQE0hVGTCeF)y_7oby)9T$iLF6Ny2+jd z7dZ72zxLm?CUjYPc!R!w_2Vo*(2o1NGnr?wFt$c#$mDA9R`1?G$G^%#JhnOHJkokk zO)D5t9_8fMB^6%jKAUOiJoDGQfbrMz6*g|d;eeRKJ1O`S@VRizA)DL(a5Drkpu5s( zSeIFRoPY~bI3NS>@;}7}IJQNbo+hY-fKE(Mt3Qp@j|69vX06}<>%@ldii;2qV6;Qk z3q{0teS-kM)%^2n_%waRf6?v+EHr#M_zwOkS1&ly=pwa{18>#P4nhj&Wo8a-I> zMDK#r@2FLuvx0YFT8ddbt!n&DMBW*ubm>XFU?Jt~f1T6J;e_7t#|uaN+~63DC0=lg z(6_-~Hq)Vlo{v=U6gm7Buuun@0i4oGgQJccq?o{F!94=Fo-8TDODknbGk$(22EUA*ujU^ zgKO2BIX%Br2o3zTH@tBj9v>$>cSzOop8`C~zXWCatNzf6CgNii1oW0z_#2_C;E4_# zbd4TMWMUAwxXiZzKR37mZHX7`nj~c75bz%;+yPE4TDR9N;O7RDp>lz1_j|EGmcJ2E zm^R%R} z0{;cuh?u|=U#{oIT6kYNgnevth-;j;IXjai0uUcw=RWTl&iVkC`_AW@N&<}MTZNzM$gti6yQw|s^LFNwnpayr=kr(;dT8=xQU#bSJ_1E z|APlP5yAm}N|z$=_xjMdz+B+-_z-wNU4ho(1!?iT{&=1_=wQ+MuAsIHKR0;$zpEOo z_Hr8v#|mI?MI8Rw1{fh6aM@vyj);Gb4c8(*zFXhgINE7xxULu?gK5B!2Rcmk6xtNE zZYj_2uRn&g7bf9sP6f7?hnN5#(&`Z!m#B z{;c&^=+`9j$nnrco;hjby)mk1u4z)AIyYxm%8o$9uT6W^A3AC4Yk{uOLjbqC7M+Sg zm8Q01mFD)}DXhr(;dybM4v@`S-5^IG?6)3Zz9+(W|yn``a$=vevN6qV;-hv zp!dTT>-kNbOG~DA{*#~gX8ZAs4{QF1HNg{noploRj{k2HQ-0#J>)yI zTMO`e?w`-J>WU4-k`YN@kwu7nLocw-dOaH+5dr*@H~UbenXAF=xZ1&?3Tg4&A}8%M zL*Kw6jcUU(l3iwyA0I_aU#xg_NRrm+NedmsFw}LJj1IfA95BbE6vS9|-&v^cKR2K2 zMuY?I{|ooh&Fb&p3woCRtIUrr>g|}!u~|X} zf?rb06!Jz5xChGKPEGcGj_4@>%r+}wVNwXA=-MO8E`#L9v%~_V!y*DVu;jK0Pp?-N z?YG{q*5RRcxEh*p27Om$%8=m8J^1yhyX3zh3?(+)4>BO$yaAR>oAg0-kGD=Kfb3)o z@<+*|PCr3^$U&;TSAGAz26g8##M9wN^d){Z5Ssyf^1F^?U4(BabreIhURAEc_mb4q z)J=zrTkOksqc-!`f%naVgq16CY|HL6LIxKIsu3({*#bjbTNkII2jcFC&6J`z#a$mR z_j^l0S&7`b${AoYFf$6%rZC)F%`J3jqigg82Y2>|>e%hJu?u-azo295vL;LSf`u{X z=R0UHfj#3XoHJ~>;OG>Vc$YxZ>OGpzKp@$(=_gJDR*`a zeOvXR4G`ifRQ{ya51>(!ro04!zQf^lb2x4_3@5vTqk?JsLhXVRg2u@sLwwz73d~5a z&kCdP23ul@LBWarugwRctp<(OjJS+Sq;?R4HdhAtHOG4E=`cbkap`itLsyz}E^Aft zSjcZ!o%yvlomx7k{5Tfb75|?4i*{Cxo+K?klQE_~he=OPK}5MPwpI$A(#G71pu8CH ziE(NmhxH}y#_%swF2idtdxGu#D0r103~&DGv6}6pj({#pvsrphr7P_I*!V8s5SCPu z$B)sCo760w)Edj!|G`Q(hb`=J^2zqg*tj^*{q&+JIy_t9k5rtq+R;1DKUyg6fQS7t zP@8Pxp;A6+v*-N|ccZfo7E&OnLQm@}ZGCox&2%<&w*NsfjRgxj6if8+$YfOjI;XBS z+pO*5dG;Vd%q|GsKjZQkTSD~!y`fwRjblH~-Dd2BDNWbRd%ejNA?r9k7*#xDWhq@t z`$;?7Erw>I@EnC9=n}?EDyZi;5OYd!-=K3wFZJ+F7pt9UJPC>ZVi%pgoADg~CQHcj z?s5y`!vBhQe{1U!_wVIbjt}m4MXGU7Uyt;DyQX?fpkBdF6q6)^54NinWaiNdG*DEr z_)GRE(~vhJP0=a-99vY6Z6J;U!zJ|hV2e_d8q~s{CJFD#ewnI8`{`YHe}B8r*-V!q zJDu`Ze3_se+>gbf4?g+(3hd_(IUbv4m3+8&bYJSzaf4a#Cqty4>A5~a{rh)|-;!5- z1X~pqxymHNuj%4*8iKmxvht<`-ImRI`)xS6S;47#I|~*2I7i=RSULQ+2bxm59kzqH zs9h#P2#DKV>8EWpf&=|cF~wQRQ|YHwzCpug@1I!SOLqw`HWsq=htk`>7T5ltk_}+W z3=RVcoTEJu91>`d=wj>*e3|s)cJUh#3kpBb_CB&g_$r>12nzb6;+6@cu|;#r49BoWnY!Wt50 zYIk?+z`lQJCOYK~d==A?PwjQNCpb17!_Rz|{2m*(rXx5m)>}!6NA1;^jm+sN@XiZY*3dh9b z{6xU~;GOXV!ce1MgBd@hP&?d_YJR__wrSH*{Bsts#YRZlX6cY&-C?~W+ z+>iaym{*WVhP_-%?+cUgwhH>}kOdCJ?|*%xuu%h`_UQ^?Ix*9l^f9QjihD=FY`N!{ zmt)vvD-d|sR+7H6R)0DiKHeGlgxn3)UlhI#1xCE>H^O-SF|n|<^|!&sp#7tpZRnflXOA7$gZPgx&gTpWJ>8Hr zMKW-`Fpy|EN2IL5|+gj68kA?{nWB+&(ep;o0`Sj=STh$%4-4d z3)9=XOfMor@9BCrvRmWD+s=_RUYk8Z56E87pVa7I_S=8wuq#c)fy~-PLn~DZ32bRJ zRO0f`Pxb7rH>4q(DK)?11U=gLgm?nTZH%I4&E9sUp%GIe)JtQgwHq7Ij39D1s~j7g z(#1p=3_4`S9&3a7*!3~lS(3l#!V;LI5Fa0}F@ov~jO3$ECj%URxQsPel|kPJYzQoQ z95Q?dfvb1|;%*MPm)pCr34C%Y-Xf5$`wurmAfy+>*X55`Lvc$)Kkn7?!A|e8Ah{1` z>u7!X!bz^D7rq^V1ksJ&BpPM*6nCD-@jK1T`0+E%p!HgtVe31N^f17!bN>(n$yl47 z+>fq)S@UMCs6k+LvorLVeAL0UHYe7yCDD2`a`w z{Z@bl|Lc&!&@V-)!fMKV?#$7M%RDq*JPtUjcQ@NM#vifSjJ>nFd~0mgvR1XilSk`X zVu-T4AA)Z@7JKGexyp&WKbu7F++92Ys==5;eF2QHKXXLMV_8^O#?e#}CiZlS*pxom zT?6%@CAyDm9))j14Npm-b3e{(d3If}bLXQR54GN_M+_xaE6|@_Ltu^=eq>GCzfpZJbe5uZK0vYlwcSZbP7 zKlr)k@3#o*JDbTHCU)SF1t@vg=X6$^uSNq=&fQ90`MB`z?mELDXB?P4bQ~WP%3Od4 z6rL;T?Pgzp|BZ=y=;3s*Y!%qlXG$I9JkwV4P=&`xXjTI>ZVzL(sV<+ml)x(QAEX8p zyGXDtc(5*hbsbCBT)W)cwjl?$MKld_MEXdxc&Xq^4j+9O1hcRCA+oc6mp%k94}fRy zyYHEhS>NOgHvD-1?wY?%`8x}!8F&~e+#dISv51TXqP0RdSe{)q&{lZO{9~XQlCVli zTSI9Hql%4JPwfhPNxZZ9l)0=?Eh$b*DdH24#`P%`HvyNlrM=DEnC%^<0oYbpyOp=F zznlFy3hUPsQ4Q>1m(0!P52jrry%S5F5?Ke!E1>jCYRs=F04_P}$42~&O_{~BTvu;D z^0q*-C|m|f`Vw${R&D~(QsA?Q$RZo(v*5|4KK{seG!EZ}m`R-oUk8E=+klt|Q)dFr zri4&zIekj8!S=T~Qe+X*%cqH4f&M3*pHw`Jo_4Z_4&v#|1VPRPrG}`6(eP3t0 z-jS@K`%)thBfkXPo$qFRZzy_KWmJV;YRE?^kk$5u1?3XDQw5OjvHGDBdn9)e^&Gq0 zsNmSP5%e6*`-74u%uCA+7#nnX$jV}7X@{libc%*+!*|yBzJ%LKIScVk+Q~qFiK8gf zo`Ekr7&9Q$IYzLYqCfLZYy!K9y()$foxq5@1*j0_B;cX}9P^Xf*8$hj z45U_`LW1qnX4`_kQ@Zk-xn+j_CA{RgO8q`B_Z!>;->{?{- zp)Fk95Po_X%QrA;W&<_S(;pqGfk!^F=vyPI_OPL9P+fm&NYO0P!ATtBZ$^&+u#bM- za3P5xrxC>oh|=ST9Ve7yUDOf+c~asDoW*E?XXNeGI9+f6Zme|pq54kRj(bc@Zicsn zxdvz}$XLd$x>h2IV*`4BMskFa2v5X51z#=>)?71#6kko8Ey-S0-PTXGl(3-or$RpOhRC`Uy066sbbTCacsB1{`@Lfd*wlKboE8jV&*h!+W zMUSpsYzWdyeKM{OQM=TA?DDRAJkV)BW&MZScBuKqkWe!=`Dyd2)uZyD-T@Y4H+dZYWb0gTTEZpY#4+)lDVCcV#|_ z>Zo=8?FYwQau16H8-7^3K!ZNUEDb^WB73L^s??(Ns8jU4f?miXrFcvw-&okd*O#6f zDCk2TRqIiyO1uhTy!CGMw(F*e>hF(|e2fZU#ckRYih5N!Ha>CE{b*VnsRcBzQ*Ejn z15QEiVH}jKVtp&?_V7MkrTTQH#gd!*g$gz{%KpVu>=Zhfq_a+yP4yT2aj0P%*NUFo zQ0(Tu^cSYvlt4Yko)I3Q&Z?4H2wz&8Dk`^041EmXT@GW2&PEV!w^b~SAaDH_7NT^0 zwJ>&II%=7*4U7td2yQY|9B_eu6>$n?AsC@Gb}9yt)!o zB)0UAn8UZeBkc93uMiE>@v&F<1bskoEpTX9l5eP68zAdt`u6rBrn8tg!5@yi)c1{?Ge@XSDC3Y^;o}X|q zbnQ>NazNHXW*KJO!zglU@FAW#EO(c@SlOCgfNv-MZJ2R1Q%_omCZ_~yRI-S65~Nh( zy=Gx!#J17AOm8I#x7#=Zd8eCxk`xDUKN=YspYlhS3wd4)@M-+ zzhV%zy2l!KW9*+jxs@aiAhiZ1`bCRJH@5zEz4(e+uFHYaV2#^)I{GcL%T|GH=U7cT zcP``_dfciLy+0NbJ|(xAD%|O+prxM-bT1I8@M{Wb(zUpoFe*78FX6no+Ra8hv%6$B zH216n%YK^5_sg4WzhyvH5knR z8R4a$vF>_=S!1SALGIeF+!8%xFcdT7s;R@eO_l@v^#SlfEI$>)X%Hy@r5MX2XoJ0l zr1zH;>Egv7_5^{}*hraNDr=Un5RGSaPRr9~3M^Iflm}Fltvh+Z{I#`GGBr@B} zc+l9a;})6dj|9$l3}#yG+m&i*e>pHpn@v_UO>P=8CK?G#73(*7nG~9H2@e95Hwt{Ta!FyO)x1H2-MCzC z$3Q6J--yaL8V{Kt*3&}~yS^WTR0BcHHp6Uvzm!%l%QW z{6$2UmCB{sQKYHrSS@`ZwEEP(Eq*YRLE1kHjSGPxR{~LMsE7k=eWpp%9ybO+ zQKuE`u|=Myxxfuj-kW0ZUt>05{eYHlfeHPD+*^5*sNGtw@=GM&jeYC8Yz9Pmngphb zGAgEI1d}XL*S0P?;AvF!8XJ2R8gZQl zKYw8yYSR%_t6Cu>MCT14iN#ln>r;voxV=txisq3{q?gT*rb*|K(@d;HPXCj%urZXB zvPss9D|@T81{{1zAQ%dGl&&MB2km%lO#8&FC@6lfl&Wany0`pgfUg#y)de!*2F8cv zKp%G{*IHl9W5d4pbQukg$fxoLZ# zd83q+Yf@q>hq4!%=X2*x89svz_04G0$qIrSADlp{9Bc*Y^Mt+WSPbkqX7916K}<%o zCpWP={{7z`RBYrhU5vFWJV1O-H$`lUE71{lPT)biCLE;7qUXeisqn5K|J`rfXkSdg ziWwhPmOiQ*v+WtTYS=G+6Eli3h>L`N3fx~UC{pBC-uF)s#s5&3-`H(JR95WDpA5;B z>Aq)MWfM5k(hwXp#58I!VI`vbwe&4E8h>a^m2Nyd`KvxIa3@`X<0FnrU1pk82?gtL zo+4OrzEKD7QgYw%4$GFaMZuGMZjVCXGbABfibE9;_Y@^&w`VwRUWc<14Zv;i{G)i- zu`KtUqYNBG#K87-=UJY)u!$WWk_}#`{u2^o542Zt04d-@am@uCI!zniEZBlf%>m1ssl1R^M)WgnB=k((|D|V9@6j-Uj=QToLTf?Oq~us z!ra4Ki{oX7=#3v$Cv0ze3mQw44OuMm8q@I$RY-geB!$XSqajqYo3OKdMKVX_WtB3& zvKgX_$Yr|~eJ`Ady~$M%WAZmkJ*~AuG9gfcYQ2!KRcdW`Z*H8(iweFDKcf_h<}DhiO2@}A`6~LfflW&$6$?~H;K|X zRgdqEM@fNxeXamR0o)t7K0~PVnbB2?Ywlj|&xY$Vg0|@nFod&;B%J{(k%02!WcpBL zdCf{u+R%1oQT;YP=@ZGnbFjmn#>5o3LCrW%6W?F!vfNi zZDkCjX;FJS`gJ z?0AZJaWSburDk8fK6SPH>GH>$-Uz1-7~3f|L~`K^bdzsIat^^=E1ddcb6pJJ$g-Vs&9Q(+?G)*RW^z*5LB=i$qH*NWOWoiwW+`Rb$o!DlIEs}Q zfuRT4LN<6X6ekLhu7nP$(xjs#KRHeKDN;7V{5Io=>}|H?_;Q7`j^dyO^iw4;b_`$c zo3w8j*~H?Rgsf!s#^E>(>i0Lo4}Tv7nfNPp=A!^@@@7-VDIA~K?6kHnZ_gn3_wrT< z`Hd-8MV&c*1j!>=1RFt*1CW=1mYajJrB_Lkxtw2hBHpkR1RIs@OjI$<5x7m zYaUTKcPyOWig%;)SZ@~vP7Bl!R0zPQbI1m~(@kIgJ)Co~PO4P3w z;pj!o)B=S_t|VJpnd8mg1?`Y}@tt$LYx$1cI-cNw+4`u-hTJL{@bLFGS1<~IvJij0 zN~?uF7j24JCArO&49R_U)Rf^v9k4QQySp9EVEL(p2{#r@q3QAwwVp$}@nj1RB<`@oLQxHKP^}5@hL% zEszTG29T~zR%VXV@ujEk_@e+KCoXekDt-C6Wg2VhYA_lXf(l&^^wVG{eYGE` zAq~$QcJ32WlzXUNMK*k2HkZx@T#fM5LDPzNCaH&TU8W>Q3?ruokK&rcdZ4{lL^WNa ze+=7dBhwO$MzFTG7rSAv-Ou*#?-_fbTby&h0!7d1c3P#T{AT3A8eET2xeug_ot2jA z8p`WJjbK^uu$E@@eSX__k5etJ)le(rjqx)rFV zj6pU46W2fzL{Ht=Kof3BYWXAN!}LCOZUGEE$e#cC9dRlVE|!R@lKP0@n5n;gcJQUm zI;DQ0Pbubb#8#Uu9Eq!a2h`Jzuv?YQ=vXkDuNVt1?}>1zjeoA;MPSHfL}UXQfDi1L z-FQ`l(QH+E5};D6;)ey<46xjIn^K3+!BDn(Hjvd8bWAlEP4?d-k-7;~gVBgnU&R2P z5N%*oF^n+)!G*7{4kx40nmfS4pQTDvTN6771 zyiYvn*GD}Aux)HKOFbkZECz0!*ol%OAgf!I?$f=UXc;+zdMc$ri z8${KfQdDBC|A~!%OMSbTVltd())TgMH~M;cuFgN!OYh_OO7d-X%LTHYo>F7A0@l+b z3@K(`y?4rq4Cfxo{Bz}ldYfNEpROp9rGE zR&eT6e^mG1r7-`4jL8hD{@oIr^$?#Y*#&m3(4*FyJ<C(z@vfuYbi5mFUx!B37Fky1)Sd&g{Hq2WrnFZtTR3?s z-n3a#eBx*p!SM8_T_!Kw83!N!$E$hjbKYj6r7|JN!~m2R+lE{iy8>R~SO}C0tP&CH zq}7ukGs#ZOntjM4Y2sBQ&B=mE6ycTVJx#-J`EPz@ zA_|gx0tjHPdpkIuPOI1+-<_ood8+SV+oHI1icFuQ+-IAA_hdV{!Pb~hM9bRJN}9`s z_TfYim)(MUXJo`}j(CB^S}=2bTWt7%OQf)S9n}%|GZY}-q@}w zF9IHexh+hxv~USF4oZS4uUg)T#$4@bNxO?m_+i{-9))^>XS!|6+~V z%s>@bStMn6hI`KSc_K;?y492YY9@T6z>PIi!h^gwo-&d+f`_0SRV6H7&(UISWq~%O z{Pyg(dH>CJB80zOn~+(nun)?=dQAfgXEO~8TfLPPW+(9BQmH{aSR|)w+Eczc%O)}uwm=`EK$R1_q0ps6A-Sf#FeX81iuMnh`A!| zrjYD_jA*u`t~}(6zE5|)dp;)0?hJ@ek^-LdoD?ifGr%O{9w)^dSpVc*vX|b)J#JTD zZq3;si=i*HsqKfvZ4HvU4{Q6#cz%yaOH!aVzZAJU6+v0WXZ{r(!JeV_eq!eLNZKmG znVCVr*#Zba5Tmd=Z(oi^H3*M^K`9q^fS>GZ&g$6+R5aP*PSr-o)e? zcX$atUNuoQZU1~dl}X`u4n1wRtsoppB*0t!(f2pQr_oN!DGT@SMQ(CZD0F4H$LQUU zlr<4%@`jgDYh9io)2xRcK^_E#-0W8|0I?{5srkSCxD}blMw#+=eqP2DnQ-fL!9BjD zTA9(b3A#=sYFU<)*h(26vmtI{fI0*s1eK!&H99Fci;Z4T-F6)u-QHZFx}2G6e}}YC zcv7&F=?s&zR1!~^b`*ia5nc&Ie9Fv{&Q~U@aSdg!PynCfPPuJ7t8IF)VGrNOKj|08=t4lqP!PD z_g4W{cQP7k%_1wDL@h1DFC$J7$Giv(=PCd8@<(wl1O{vcWT-Tl3HZ%|8U&CS{&BW( zzI!aF{mu+>JM3e=y0Qx=!C1^UKqI1SYL2>q@r9!gdgQv+X{C1Fy$xsc zqPOVm?G%k{pu5RIkJjmQKLj)FLZi{%|!Bg#Sw4Mw=Rj+?x$DyYIQ9e%eCe9pU-1egVAas8rZJU zd63P)0eB2!ju?b$8|@p(u0Di)d49BuG0#RBKqZ1Ao6`wH><)aw{T{++cdC;xBjxfT z#!Bk1GC!AZbIro_ch1aD%i6={!&|J1In+t{lL?y&t~KwZ4Y)_rVP;X9<$BsNzNo%` zNdZ%))?3&{oRmKt5|NiKdZ4{DMB>17G%kcEyjm-mB-zWcKBsI5spmE+Lp()D;LDfL z0&Qx1X1#XgJP?X3!6bNSF$<11zWr8O2;4LDSx)&aM-Sv35wj3 zSK7J^6($;VI)WZ=vvrZ*Bhi3mZN`y47`dPFBlmKeRhUSo%@y?~9&QV*etkCMbWTJg z#zG(jvVp!>bGs|R-I5dH9t%i3=>Bu3s30qY&Ev?kD~k^1^|R<^Eb^-&8D0JPK`^)@ z2v9|~D2eOfz;|OG!FPD#dU;Q9=VI{5QP{oTrysQSyUe2s(GZjs+d4ltN@bZ}m%<5J~9~gYM8MPJ`b)@wG6}D(cSWk2~gtGuwFV zWaEbFA2GZc&VQ4qp31^tyBIEAijQ2+v!_@CsXZ?pplkcT{F<<{@Gwvm#kVrt|Lb#;s zf%ekv{mN$t^2z+BWsE`EHvp#&eB4zhrT))!Tn*h*;M0*qv}OXSx{)>>HcTylZ?s!m zI#zT8`rm}sb-uQ`|1S$Ljek3BfR^xJrg*@*v|1Dv(8gEya}u)UnID@-D8ApT$Qa%} z2tPaEaPh*--5*=~S8%un^tDev_BExx!i$W46$3CO`{x2NsCuBihz!e^;UmyqOoqe= z^BZIXaOxBO<*R?G1Z$r@zPDv*xaKhPV!ZKw9#%v)(1g8+FW0|sU&F0}wI3h#fCe=6 z`nPlk1g60gWU5U}bDM}Z!1CYBwVP?Qg4{L0YN!#34P>F5aP}{=o%#xa5Xiu#v?g~z z11h{5>AA*8rNXO)AhvbLxaZYD5>&pxy;~2mYQn1+0A3~^#z{Q~86_c*utfthqKP}R zOYt=(X)x5-!Z-(>STYZR&wR7c&^*O!`K}gH+Y{C&V-3S5Kv4fR<^sSoM^u%RKsEp$ zM9}`gZAUf$4{(?{uRo$4T)Y`A>iZ689cU4Uf2ao;zP!Ck4Luyk^|1Hjp(q^qARZ&o z(nXI80{jD1-e|RuNRzI$wm^8138>&a=>DHk2LC@tiRkc;O_rm$dBq*S4O zX>;6(2QCk`@K-T_m^AwsvQhzPUYeiWS}Q>$LBhO)gX`O zASk0^uyHHJ=4>Isk?+T-2DVaF&@FJG${^Be)Tz$t1!bvNkvZ^%Z-@IxmpN|i*SbcL zyvgI-?KDW)L@A4b;wHN#kGRI2!d}qE8PEtj47>PN&dSjlND%!2qj>wER}2*^;I)GT z89tja;%|z!0)x@+eKgyCwu2#EwlqNkea@`@_8&Ea2FfD(l8e7(66lT8n*xeqz6+l6 zahbk#D=$>g;dsuYCR+`H06@I_EG`G}N;e)l0dv&QzD~hO^Iqc^S{Usl|}n#X}YP$J>)yZZ>~A@s(>6%0-J>!j4`E&r$4q8G>se zk(Y8K6mV?-qt-&W;(d2CWf{yTpj*d_oVKft0?q&vg|CGR(tMBiPZ*0e#*z?-Pluu*+efr^5znV_H+aGGfFd3he2@XAP|_&a68yN<>HB5Ii$owc;ZaA4p3R6+!6K zW_}tS2EA<*mhibJ)+$sD)8eRx`iwd`aa+yfhe4P$+cKp;cR&b3dK^+d8yMDG1JDWB zqxRp1&bHa9b)W}OuZ(UdQ08%FVww%jx9|mY1{uYeNZMX z+32KaGT zZK7frp63{4)BD4OXO1|e7Cad0HVwR0_dy(D1Njq8!gf@V@WA!8)VZFmfTt z9*1eCKSw3L)!BHvn2>J0#}yq8$E{yJs7#ac5V9c>6R%#LuC~^Uz-w( zdxuMdXQm4xZg4r&tiH{ZK!`;%#IFT176WH8<&bdN=HQC`nh) z1B-E~VsJu?LA})|_xm9&PlrXtNOoKko#HskaQ?1K0uFn=Y}qJWOIQc9{b2w4s$f`H zp(ly{gkjsLl_?)Q#8I`bb5}@vg~#O#FB|v;w)h009@N7UX+il=awBeF6AS*p_{_dV zzY5t#sj}0ev;}R4_$IOYuA5GoH{?bA>Ugi}ygS7LG(xly)US*~TlfCKjjp*~YXCgb z1QtrEiri@qvS5@p0~3TqtoiCaQ1(qzZb+FuX&0-;K9#u0k%r7u9%6Jdj3e=%cL#m6 z$k-sAB{byDL?`^D@|We@c3x{syy%e%myu43MXxG$v>{-yXn$Io%Mz++HI+^QiZ!xW zE#`+@orBtL$ry~gBgI--*-hG!Mw3dg3CZ0XE)E4=iLc+YrpGIT6IjzcdR3*_5^Z_D z<&TTf`8Z}eDRFRDPs77qE-A#XkjRQ^R>rP`d)$^lDPymQ9EU<=&*i%j9H1|Grn%GQ z@NT&B`F2FER-?Ei2UThoxk^01cDx)>sw#I8s!^70Jn# zW&__?s~(=SpjJ<#F$~*H7@tiG(XxaM+gYbKo)Q%%$)U8vgI}jg%1eB#&n_QP{S;Ot zO$L*6($Kho^*^}o{xr-K_0loG`e`%r`CH(F6vF!0?p0KO-&aXJlKLsdqW7mfG?%#7 zDa7Brs#u^hrK&kXh!x+bbmyCx?ci?T|BE^B;K39!<0N8x<@k_79E8iWncjVxT99J2NWoWmI$I>Va@m7g zM&3`(_0J6VoJRM1jBjZvYu}j^Q$xbOu>+H_Y%i9m4Su-eFsPb^NOi76QCYP@rJ(L- z>e%V6?a`atlYUdE4oCr?eApAU2k;%2{Rq<<_B0O7jv4sn%r9EXFf==xBNm`itvKjx zXB(aT>J*}prbr5(5W5QSfJ9T-vKt)i=df+S(h?nBwHbbJ))EWaSaMAzu|$zkXTYV% zg^0+O^(FyZD!*n+U?N7UjsbY&MW!DrCC=Lw3Xf!ZkS!#G*@4`_HhVwPGaySX*38dO z8j5FY(3Mu0L##%D#;SiJnHBId1(cTG(DG4Zxes@v^}r>*e@Kf|h? z54G;xtX%JK!)2pGy0e@8zM z7bcF7BkfnbDC~3crb)Z+$6E51x;{|cW0F?6>xJScVb4}YW^=Z(>z3;QlC0T@q^&om z*roZ@GMg}ysmmkY@>T+cSd71G6)T4ZPdZ@VJa>*L%8^ubkS${S3!IA3BQEhr0lbZ( z@8hvz0K6|U6aAKO&Vd-7ogWgQ9h#%GBmjD9X<}XY5i;k|FmfwZb)FHVP|PYty=Nt(z2iTIs2Vk-ViDj z6ap13tuz1|St-+e^0@2asC>r*h*@;hXWP3mb#C#8eL)>5u1k33R%MK*y;qIzHB!XP zc0$2nCPW+Zif4aCUMhpvc!qILBL{$e>D)nC%V2;R5(*vcJzni$< zI~b6e>1QPF_V0w&@}UhZ_ee-}6lIdr)RFA06UQw#$o}OwUX_8)OD*0cTe^=LHhI3e zIXPDtNm78i@(w?S%`|(sBr}`I&GLPCG3@jg_jX#1WQ+$z%0Nffixc&Y=F3+#G~Z@H zav=z}<@5t-Z@3A7S5zV|!BbMX1|dZbKInUNEQFJ3Z0e>ATjowlm~zo6gb|~fVr7a@ zY7y&_c*?%9A`e$lChYfT8|Z?}B_V?|#g=c~mNMFKxpi3$WuoytY)cPFU zP74iI(J2mL9>-RsOk;eKF}{C8!dgkIv&KS1u&*!6riH#-6Q+q7)L5{rw}=N+f1SMLDj~kYOd4XKBi_^4B;?AsGcuDMc0wY zowc9^-%IMG?#a>8C1)_SZW{YG{@)S88h(UW{(R&nlsP>FRXXtIiiu;pbB357cJ_y1qlf{Asnh$tL|kp70= z?2s>V0TLX~X%xXoQQ418Ayo$tG8kxqp#FlDAm!(oHfftk$czbT39tyNL|vkYQ3gB% z9pNm1n?i}+P`>Baq}5wSy!JESO3!HgmBvDnLGdLqv^?T)@&ilr@R$+2xR_v~v*gJO zHShpU-^DXdloe!!K`W#jbxwnz@JMyItu8ii6hsUt)~h;~b^NZs`5G9X2@6id%7ZD8 zHgXz>O0-F80M!d-4DBEOXFw+kDv`mKp zzF1c;vEo*D9D^-Q56Vx7$UUUu;>)G%Ieo|L2Kq zxKTAwij8r%X*`eXd$Dt>CZ~@~WD|)p$|aB63)J#QFPTV{F8%Y>aJf z_1;m@s0scu{?LenORX2=OP~%;TK}=m@-J<&Nx)A82L}BRnz~jOG^513o744ghe(V} zN~W;zaoGq&_yq%dA4O)s`a3Z^NpH=mn=eQMW=Y+8q{xQ%vzS1`=3K5!{}Mnp2|_#M z+nmM2xliv!+UO+|M`gRKgX3(2WYC-MIiQ;vl)c)B<5}hUfObyy0VJnUE79Yov_gjr z_2z?9J8XGzb-MDSAd)PKwv4oR)=?r+$2Xd~h!g8{E!bQuCs@ZrgC-HmRg2vPS$Wyh$ z7R71z{{uENlyb-RKlnN^5)_}04ZcBTHy%a1dO5N&(~P^>A6ZfVWEgPKl@QJdJq^n% z{FqZ-_-B7g;?o!Vo{3OEG@HKgbg?3KO`?@(IXL1+%yxqfkBKa?KWm17e%+H0_cmjp z+1+MSY!Z(--pNXvzvI_eNG}2_>c7p~Nsp40;==8rM|kUG?Y~{mpxE@CB!*fp#4zx3(|o{ix_AjY zhi*hUD6sbHzEW+YOs=chTob@yZ=?Vl@JYY#M;*OpR&gPT5?Gd00K?K_kIM2=%Z^18 zXRY0<_i;tDH{bJDl6m1;C6d?UwYSYN$;N#$E;+?|R0nzcx2FhRp*f*VU(v40swuI% z7?(_b#T4Ne9`_LRJ^7HY%~vE?rCL4ETYBB159PW|ka5B> zHh1Vt?!*$miKlb;nP_RXdH3;}gU0uLh2Qq{psfr^0*3In5AU$#G{3TiT-X$+=|T01 z-}OJ3{PhyFdM1d&a^SF6izJ#{Wk1Fzl$AP^WZr7l>Vr6Q>8kgJ!*vkE8G1x`<}8xh z_2lB-NvlNdjrsJN2z)-MHTWa<7KGC)@^FxceSZyif0ZhfV8tWmZmK}q+s7%RG+S4U z4Ioir?dqBEjrz&YxXW@Svdgp})@GSFmLsziuvyYk%aGV3GWJL{QLU{i_H=o?=+o84 zo2fVvqt^V@G}kLu%uwxV^!M3`AHS~zV|j&U6UzGvT>_HZDLO2}(b~6aP38iyZ1Gne zCpLp+41HH_=i8(Zm-*VSuV}-FVa3NB!z;K(4=Bc>dy_X4I<6^8==put1F!ztD6?A2 z1&YJ&S8UlJx#-PNwbl=v!>g$s-4i>RVohxS9TFg@Y~E@VdN!N-U8TWl9HzjK#5at~ zA-@hidnlWj&8a}?;O0mj1c_j|^C#L-v17Mz%3$P-gcI^Bkb{!NQc*+D`yjR44^;hQ zJ}DB)@k2GVPk{DN(<~hXRtz#;5q;MndeB$fwXm#*580yo)k&uGl69Yvxd& zg#56`r?iN&q(IoQP~wiZ(f*;@#qoe+U$IaU^;7Xl0I?Ie_a#?zQlJ%&LznHGxjm~o z=wziSI9AnXqVnsjs)@@bPqY?<`%eZS_zG>|Q~43-?uCp!nL1er9xL;*vveE_otNrT zU6kF8I+>kp4>3Jqk-#LO+t0J8xvCe{I|IzcUXSNWYVW)rZi9c;sAdZ95J!(+{u;-l zizMw_EDMb8?~-cd_7C7kKuv@bp(Ae);SVF7l~5BYo+)NEAj-{_2PkzBi7ES}=MiC?6^sclgBN!2GWk-i_a>JIo&LEpRE3v~I{ z3#=ZGtf_nqKD|Ybhwt#pJl)>(bT?Z|MDf~s#W#T)oq1oLn;|f;X=-{(7gm+QVNjPZUULG?NPdjw|Dd6a>r87kCPKZVFliAiN&wQyW2}X{M^j6EgbrVwu4cO}2tWzwDgwy${tzn? ze|A_&ZWspC!W9UgeAd=*50T?T!jCwaL? z6G}){w&>jbsJ&!&+d~lzH#o~h6GPJwt^$s-BZs3vl8s&Zl*omtSYv|_mdv5M%!nkm zHxN%q;+z^(OT1vs5WxEDTSLwNDWf4iAnuH#sqA3k1!hW}5cH#QHYxOoA*Xllt3&)7 z?fh)Jx6Mvl-|eWf!&}zMmAPHdA=m(=Jc~RRcJ}yClUrZysv8-~qL#vbtP#TKT`Q^mwsT}U?|ASXi3~Uw$k+*?jVreeq zaMR89m{9)=b_NDb%-r?|%p@J3V$Q>0U1jVq)>q=$WXgS*_*x&XUNex!`MqDSe zbMm3PQnHKW$){k&7tadqmM(1Ql=-2!d@cJZ6LvQwy>N_Pz)k?J~J0+abK!Y^f@dJ1GLqm`CG(An4hL55XdHlBJR~ z#h=O&SyRRDcAfl}qSsEQ473Ktq-Tm#p+Q&H?I zhE=d+1SXtykMD%ztj`BZQeYyb@f#}vN1j3DLQH=@1D4$pBHPQro+=l%>5$F6{{jk9 zfR^U;iQW(R`q#{rO2ZEx%ME?jZ=vKBs$mvp6cgH%Hvu!{tdn*Oo@e zu-qS(++~Uk{rA2bNB3?70^Ignliv&56M#5nLVXYZb!7e-q{@=)`zlua&BvKODLRaE zMFPM}_>oBcf7@;|9rCmhJO3}d^7dBGsHlMnwCT0lh}Gg#UBVGCZ`(Z)G@Xb~p zYT%x;T)RKC{{Xx24|*E6zU0n|e=BR9h+!Fm>$gaKYf>j!La4v0q54q0afZ#nS3Gbq zPcLHd-=4k=TMwE^7-ljP#6pr$bBLGviieg0^zy=t&ROf< z^T&1iCCiA8sey_15qtF;Fsh)z-5b!HWWiG0XADR&TM$k552hQ0A!@gv5C{bqzib8! ze`#ZB#0~lWrxf^?Zz~e^JNaUS2{i2H=4xjm4C(bvmZb(fCk*u7z<~as?5p++CJ@G0 zYQ_NKHc;=sr0^!f(Su@Bd6;;<3q`I;*8!y_W3w?r$EUh6ZFSpwej~|5EfqnMvP%jS z^c^~MHuu67s<+l=4mssl>fRN+EwWWmVpa8`3sY<4SadhRBnkTxh!yhTpti0( z%+t*la^W|!)i#ksF9fBg_JTgI`^N_oPvY7BlyO3{$voZzmpaAeKW~rtuxyun7z-3O z7IGx6&nYmVTm6A@4x00O1HlG6gE>B$d}=gO*=MOYPbA*4?#?;(1uS-!8)a31XI2<( zrY$@Z=v;2vW6=<*$N%LgRWJ#B^Xpprh5`Z_5Xxu}l1$X0fIbd>scTNa)^t0cMsQ$d zgYEcKPI=+o;hsWV@h7Ae@kr&R^TmaCRhBCj+rx=uRD{e^+^Tb<>0P<}d_DFjQ}2;G zxjw(KA6w}>Oya7nz@|h6QVZ^;HtD5&;PiUB^*4nnZ)dr5CK+Rjai1h^EtKg~gbnSlyDL8LXNZ#Sa9%q2s-P+;X*&uaDHK(e(CDlqtU zFpy5X)dh-({_`w~*7Ien7gWmwTEP0G6wjI=kmcUCSSgjRrS_IX9rU$SC-~xzNse0% zEq62D3xRKsofuEYlIiTmxf0`jUYV+f&^vp^Im#~)sS~4f%h4kCZcqI9Lc)eebMv^i zx@~@6W)932N^EmYk_x8pzgpH_Wldz$<)KM)ue~ZWD zeXznZQPu(%x$~CDk1qFZqpogeBLM!l{r(7y11FEbVfTjf$<46|A6u?%omVegRtXYUj9*)lp06Ph>w z;sRK3UVL17oVQ7{J3OPAwmLwnu=%EyGaav>um4MF4O5`~TQ|hKMlsWfK}pV`NZPGvhyY;EiQf05 z#X~{{#43}J>k1sx?dgosr0YGy@l+QidgAy3FpP$zI!n&kB*nV~vFv6^COb%)ScY!3 zIAjpL|sn|!L>-0185lQf>gIB7cSBX!8NarP58=|d*q zsAjM8`t#v*{WpU#7A7LTk5;>;*U02TKm05XkJnmJ8S~|Rv}xUCHBJjEbEA0in?4&4 zrjz2Th>>+b4{qK?^@>&h826oSV)hU4%hbhH$Q_D9jh(aY8?7s`ca(Uc!{`1I$RJN6 z-C5#pn=2dpGEx3Dg1TnsbK1P@m4%{0BRXbE1BxpeLCAU7=~{c1spor~ zb=ecqZI#^eFtascfR9bKQvYb-QgvPSU>k`y(aw}g<7>=}4z(VI0fvJfgeF0y49W4h zA|>4Stf>;o2b@AIAL*`+`=$Z`{@mnV)MmwQT)^^=LW!)KKPISz^<=eLq=L{{U~!HP zP71tDuQi-W>S4E&?E=ySVT7z0k>!u{(H%{h*1qCI5{mQ@`mqri%Z?|&YcdpOOm+KX zwr|mh$F4saQjS>b8t7=P**s>@BhocF`s>dr-uYQ+DByI>uz8IuS2jh(U$=8`XTDJn zq;Y=adxV8G5rS-ylNd-sAE0+2{tfD%G1$!Wt~c=x&1reu>o;57pBjtXPe&`YAhDqY ziPLXS*A{OQ*YB>_a^n+G0@CYw1dWyD-;zEZ&SOzi0yWjM4BBoDauj?5tC`K;9Q26H zFE#v9X`tQgjbs)5v7Y<||3uMNCmclYu^ydIme+LY zzUZ(TwFE3Oj-X!Av%|@ekQr|XI!}GWjSn?4H-+me8TyE7<`#I~)tlWT^2G42(&Q53 zNT-4JM~u=kWH<1XcP7XpUO0-lOg zY{+~_?kv|ti0}R*oMg1dMJQjL%&`Q<*92m>t6R=p3^E^0RTjp@diexdwGXe+2!*^q zjt->H2_4@m(t+qLl~o&vv%M-%V--uakmB8u8_YtY0s+%#*&{Lp21C^}h1)0ITo1_m zhH?%tc25sH908!}MSRLMuXj{p1rG2Q?eeR+eun%-M>vkZZ+>76M3IK$Z_XJ&)i)xl zJ!bO#ckkfESO$NV*F}u*x%GlG78ln~gB_lPW%|%STGHNo#*~&EP3(zYaMX@>Pxn@f zGh3|_x;?><>v&82k(CZt1S;NIp$P~4arYn8%k6GF8c=fULS8JA;A&2bB1g+y1E zJWZxAExe^K4lKi56$t>!T(?x(eX?(?!;cr0Q1DON5fpoKya(&rNahS@!ip&WC?`he zFyDCYRK0`Cq3zu4Dl7=(B}9Oie>%p8pJNTGjyE~Mx8GACSFu_QyhjS5KL{t8AWWI7 z5PZton#dJ%7cC{6@pNduyEr5)z0ij6@!mtXCp@E%*<|Wd3-@PV$Z6gLeN&E3r>T=u ziR0Kmf~CxwuSoN+iC#Hwy-65O2`Nb&-B?+glT279ph;uQv^=HA)^^%4A24+JH;pCp zhjNyfZ}Bf)7mknwyRPxaG{r(mDGr-ec@F!>VbptDNx*oU=gY`|L}$AA{G1lA<1^Rc z@9e}v@5{-}Kaxy%oeDjP++rlJH;fW)NnDEDje1TL(S37_t4XLW8rq~F=t1x%o2|zO zt+dmOYD5{lRFO>yd+;Ke@EmrDD^-IT2Q@U8iz}(g$u!q@9_#w9%XzTJZ0t)WWV?5( zUlh$FpUCHx$V-&>^os+QTOp!o>6<_s?KUQM9&O$)Xk^_#dWad7a|k|Qk`$@gerE#6 z07xU_y3Dn%Mp2p_andBh0%YnA-dH+ktcjPVa=pqc`6M!@WLT>{2kQm&<|gmlq`=Dj zLxm~anbg~Tb-K1bHAxC%(`@#r45U=NC@qbFaLqZ6)H%q>CbxVP1IN*8W7RW!^f91j z)dl-TlKoJu@Fon1O!J1(Vg9mRP~%Hlx>!7EwL08{P=doQ)z>@rN`=GdyEFcq?J^~R z+%wrfh}K(ecp6M141rOAp2E^YD0`(1SHqyfnYoh5Xs2s!;nbk)mdAT_6ylb6{0CdfUFIeNb=*^f<%gdX4Xy4Da05tplD zR-MfBi$n7JD_e?}^bh8a3CM##M0M|fex|ofZ;eh^YhcwCt$Y_>)m{=!qk1yn)b1q) zxLUkE+m9U#o}YSo(Vw>^v9U0Ue6e(|^yGk!^g&si+JPZ^@@|cRUZXzsT0Pc9?d!E@ zE{0-^3;7VdI#C>YC~{Lx_eHYcGPWKR`~C7wQI$exQZGUiYIKX@F4MwW_kD3_$0r^C zcEO6#k?0Rj8Wd=gi+Pbo7nyEGMZXJ`h=YMNebAxED(3=$fQ!?x?@lCo!?`!v%(oD3 z6EoWlPMve_~%zhchAEU#{Y?Bl__v-P=(3~s06 zqF;33`l_9@!$F9XTVFG^i=#A~ zJm11wp21t;)^d|L+-0eEq z-~2ESOJyUmxkwS^O;l=wZdX={!rYT#9_kLd`|3BpXR1HO&odLRsF0LT#0g_ZAuQkMcLOY zRob~)r0sgao2@>DJ-+98Y3x|xnpL!eoPsG|7h9(mfbZ%H1>#ONjx~u^>*EIuIC%9E zv4MC3in+4t=gHihAD4`j8XY&eK`U55It>Yo+6b5KYAIYhvl4;9xvwID6yMkds$7DO z=Mt7o;rpau6t7YAE9v;ZIX;Rvt5x`a6ibAKS}{B$F5vp37$b9B?i1un><;@l#Z(95 zDtyl3izTYT&=30yf^a!j@x)tnu0C&Y<<=GwLmLCl_jmWNm;^w z2I3Q~dV8#DHJ;b3WIatX%RqVb?= zUr+K7UPHeqIEoRkPVSDm;uhcj>4bTMOYB6M31o08H@ia`vqTb#4?qmj@}qb8PH;6X zr-_H$Ho@t%ssGcjio4pABqY89qO(P3ZuCHTje7guntm~((FS*N$uzZF3e^Bq$stADWL_UX&{vOR&-}FPILGgfCa+ zGR|$S<&By|14jTb;VCnDPp{3z-^T=^Uh^9Ccb4*bU>F{!$Rx8`4`t6&x4@ zFX0+f5JnPmtT~%CmO1gplrze5ig@kj_g_w0Nh$hI>!Oc!ACfpMn^5#t8@`01Sfq1X zzS04s3js1r)X3~U8Tm-ZUfQN&W+YKF9_ zswO&1+SoE_fWggQNV*qmMccD`-?}6e8^)$^Akw%@>H3!&_~bV)q3N6BX`)RmjIrGT{e-y2B2R zJ?@UhtT@opGJZ)NZ1&DlD-bn_3|60iU$Dzfsm;Y7fDITmW~6K?vj7Wqn^-HhemWOg z5%_iwQDAhAmgba-$^GG@7t6$B^HQfPWsX&BH)sFP9IVU5qJ~*4)o{>gy5J8P`b%R5 z>5K``m~i^~^1dPE&VKO-ax||kfBe#Fo4arEC>E^IiKL#4a89yTkv5MaCx897x3*fH z5f(wk&7pLi#lDuGe_Qtsx~Uy>St^cY3#~x77e=iO(hWLs%O6%33qy+kXsiXjykJ~z zcEwCSsxg=_aqH-VoxON{cdm3{jJtQvmTo znx^{EEk&x0>gly=sol=Wf~|NQf@uQ*zkJe`xEzXAa6Rirq`{6AsxpYUjAV}{*?)el zJQq}d#lHweZrj}MGYA&63Jz2|DF4a9ND7olcsWK}wpqJ|m!p(p<;73UB5z7z0(egX z#lF16bend@eHHx6XxH)8yWTJi*fm=(aM`F}F2Rg_#DzYe8uTmf7a#9KPAaB@(xhX( zZ&p70^-mFw(jA}IO|L5#pE4=&OR~}{?q7E}wG#kvD|i!g}s7{B?M&r_#t|k zbtm-x_u0O((Lf9m?wn1?EBy8pSnBj&M4|8dcCu37q(!`OsX52Zy8089s<-W98T?&2 z;tbKFtKUx5?deGgAAsYq;wT@RD^!pmFx~OZ7Z+~!y#dsWEUG9@XHe7Ye;N-;VT}ie z`i=su1_c*C zK~Q|4l@N^QQ{_^biPS4%3@_X10OQsyH@(k`CNbDDlm<6e$O^K+iL{C^fwo7{{#FE& z$YI?d+~x%HJO7PMS;CQk>lE+2lW_E4jSuA9g%T}E(c3N0BqIWO!B`jiWZn`h;k^_7 zS(h2#R2s5KP%bMtf9!^L5=XC$T|qJg0?UzcZ6(1gGmhF zWjm`;2XFw#CC!;O!SLL!hj8ibu94!#AqIQBB$d)hFN{gQ81(xKT5-Fg>b5_Pqr^W; z<=u-=0Y{tp{%_yD^;>%p7@dm?j%QO}N|p7f-4T~WK?gW~k6bTt)tiESe@jZgsxK_# zEW7IVq9HxN!f)`eDmT67{<)0dU-u_G>+&_U(pV8WOrrxm5rl|t=7q4BHOBr8>~k7Mq~wjoAuQ3TIne)p@7+QHPrh#z2ER`tYg~ z-ei^?R`L;B-wpn4ytdefduV86r#BV2xIRKx|-d<5>TbZFUHAY|XRtw-c`-lc`CVg$DA*#>VyS19E8@(C~1cBhn`nHSz*c8&GX3ilMF$$@|hyhE> z(C7s@50v(H=d2fOSiV2Kb>}F~E2jp%$JK+cMl~9Z<8Kucd0%sLmn8+p8%jpXIu!_V zEIqH`+Sz=dBQB}>``$bMw(MKknZZpVTgzw?19$?7&dHwcOA6Aei)BuxMQ7l74Og%A zGJN>V^OXhoYT)0*dhl*kotr`mn_Yv8v|wcra?OoTVFP|2jP{2e+_?uJ`=Ptsfjc(Y zBl2@uT?}#_t7>*)(Le+T!47A6bja)KH->5t_PLqqsm z8oi=Tp==@n9q_hnhDsxwzvm6HVIvdBAZ$FFn60O;KO1tU#~cBnSfwBU5Hys!bi&+* zMCW8LLbBNdvQuX+GqN4!FRw5{^w2;es@lSr7*)_J`b@A|D3xB2b5O5uFEb;E?fDvt zY+VFLxAU5Y)3>H`RYa@=1c0~VW=fcT|I-Ai1wb#i2DMPLCFoF_Mc(PN)Q9}`HsnMnx2MtaJ1yG{$IO# znx@czzYTL?e$NfT;?;OfB(j|jD3vadf%@~UIotbN{0G#@Igr3iBisDh^Qwh8$Y0^l zPK%6869-V(j+7%+xF^0V8FYBFm4 z0AnT7bZ%|{grpY^Hlq+CF88BnXJ3{?rV|z@EEUL8d~Ec(K9ggdW&+jz=!t)%(hCX) zbeq8M%64L(Vw-BR@8S0t!tN2_f+GM3R#U4Gt(0P&8Oa8&ZEOfjo@<{(YAT3K)X0nB zjU>N%@N8H&o|g#sP={T2fgguLWU$rUnGE6iTPEyERmF}BM`eRsKvV<2(6&gBX!78B z>wNWA%L_Zq$#apuM`UrEi)b#J#fIP2+k6RNzWvV)G5_&ONA zF;|^jBOSH6AOr)cAy1u`Xi!1C68x`%Dbl0Azn_%gy!qcf2MZ|UO9q<(uuEqmUW6j+ z9cE$?ReyNlt&m=V5Ad&9GW;~PSSX+$Endh3;-K6_V*I`u~>VLUqy^pYh=CtEOMq5opR_m$?rnADY# zh1huK$EMEFSFR@Vr(n}Ce;PPoB`4e<0$ZiN(=@X>it?;=zc>0^rHc_+oz?zqTVSDX zF`L^=Ss4CZmxYH%87?zJhRJ9OWDAk#aN@PDl zxzm;G9);xeF0BwaM@6AOri=BOWvz4Yq2K<_%dWQYwB7~bLYm-*@e_ILT|Ybc8$U^Y+nX3KlcqA$B0+V!X{qgvIODuO^~=V=C=7c(K- zBJHKg(W_MWF^w%7Qj$bAtL_lP#D~h=aVbPTHv^?+H?c=UyZ0`63ci3xtzhN%Hiz*{ zQ&KPB21Vf4d(Y^1gmlwK(}{Pw7BrJCYB=GjRw)L3%QPp1CL`Yf<`7OPwr{=-;ViG` zw}VY2)1+s_MGcnEM`~!jmDIQZ=H2ZIc+tYIBus)d|D#Y^?sj= zL03PffX#ujCv$MC$&)3uO!0x|AO@(|o(X-_h%(=|KPL#+0w{fIugWW>}B^vYqP@v!Db;*@*0-IC063ARR zuQNiKa3;Xw3;a&kYw0jUO1+-Wz}ja+K(UlSsZGf0iVcNlzemZ2>^8o3S}K^1C1LrJ z%!wwq@=R;&c`5I0JZ=?4Lqo2Odaw92iaH{1ztG6vtpq0oDCD1C=jkE`;dwOcF=OdA zgKSanW-3Bv^&xb$ z%^-~lcdt8N!G-J$2k6e)bob?$as+<_ap1fa#)B8f@IXmLOihO1oMQiLok@VGd6M;$ zxC-B~7eEzj(AkHuIUS4CK`Rz2in>jK!)m1_4AcV-c~Cx05k$ia3SHTy%6F@i$q9@% zIxYTz3K>~$(H;apMM83=s;(dSEt+CefnP53zm?v&=1NAWi!@WXPUc>_?FX3EFc7TQ zd?>~_Bi9Wx-cKx5-W&G)s;mTpWZc$v@9=tGyN+-nm0FIV{ZcH$czJK@tqA9tqkwplxye zaf^q={c&>bOm zwKFf>T-gM?cU-4Rrjwe)-{Y7A8d9b|)zB8$E_)v#MPID;eld~w5E;kq%=p6aQ3L$=|)vb+RnyN)>P;@X1Mh_|3$GfmGvO#P?c3r2F?o>kRjmMZVZ1 zef$!M@t<8qcYe`p>iZ_AZ>7~n3{<9J3a&vcfXRZrwk6ySo8>&e1!_RfB8T7|w8Qc% z+@LQlrFbT|5)cKkL8J%7AwMsogaKc3wvL-?_4`^k%TlQ!!Y?-tEV@0uK4dw1mrNF& z&1P58E}@jZo2PEIKVEh4TCDjEBdIY0+Exq0Elm>qEi1+Y5>w9Dx5IQby!e(EgFJbXk&NFes7--vqS}6nDAaP6i?`@EV|*f28f#?RFPV z5c?_hSrsAM_C=zLLvaj~aZDQ~?M4<@?NUb(lo_E-fRk_~cDodiQve-WOq9V+Nnk8r zWxOHM8x59;Lms*%GmL$K8YL) zUYRLnA-?1)Eg!7z4K0D~kz{+5o!!D*&qz5t@isI710r8>t z-cl7*>g{Ej4MlpLOkv&BxE9lhSEbQY4P_B@tf@)`o0IG2DS|wf+oOqpUp=jP)Lcu% zBQN-}mAlJu8nWG1d=9anCf9WnIGjgx1|jC0#<^DI+kUrJcUnjETB_CT%>pOQ;6+D^ zf^2lbdkOs7--AB(qNTJ>Ws5r88i}2}UM1i&JRI1fZPPM{YjEVLsg8N1CG0`s_sEsk zVn0SbP%4N4h9qlVes@xOz0<|C93~1+EePL?4O<3|`;xoV*-lbg(;VBsrdSEel!u}> z(W;LmUaqq@dl3WkPlAGM^MBN8?U!Z<@Lzuss}+(G-l4E~W&FhvsC46oWsq4Em~%U^B;2TnCT- zJ2|&Sn4_m-I?zQ={Co}*%ww#v+9Ikewt;Huo3n4{XP6$~r;G6IDo$_nT_JhQ7G4+O1G)6sbEi?ym(462aXb5 zbpNxx`Zv~=WZ7UfnTWAhQhGPDD{4*F0F9aQ5H6?USIL}zDB%joVImXFM%N`UtAE6v zGM82A=uC-l=5vHLfAD6oFw-a}TD8%_eFT7tFjJSXxbe^0ao#1{08Wv_R+1}QZ!~Vf_BO*5J2Q{mL7G6q_s{&X z;DJ1Mhy8@*Xo~(8mU&mAZ`K0X%$QSWR0~Xe2D|JvlLg?Ua0d45vP-a5n>{XaTW`EC{23+62DWvxUN*;sXd;MHq zJrHhIp0n+`x;=K;8uUaZt1Tz9X6@=(wVpv$a!~MJR>gTY0E!Yjy-;}q>n*}NVHBGy z_$)I_JQOTclp!bHfsB+Jwdiop?ss-vXNE#Fhs zBwXz2ezhr%fE+*m$uRk|nT}keR1{PngRIP8ggTW#>OP~}QfqB6zlt*GYy1GI!VQGr zAY}9uz4}4}XNdBAHVreEX&Dm(xJL28J1j;H0|$OGIS9*4lhdJkE>=}(0BH;;BtpL9 zma5kqxrv)FZpYCr_`Oyon#?62PGl~=#2L`RLd{>m<#BL2+PtXX)2K1?3bRlq6N*gF z+{@)TaOU~pbi&K3AaZMWvc_$H(c#^3zgle|g%QPvqiS2cmNV$f^fo3>9Ju(Ip^PhE zW>KVoRpq^v47PssRH~>T|NY^qnSuVB%;np=O)OCUYq>k3jBV=CxOkGN7| zrIe)RW&Vx~yCT=hR_13J%WaAZRg-}uZ;b6ot@bQ=P>jrK_RHwkuhbeUR8-QwQ{Ukk zb@(7INIrCUlSXq$0X^zm5RBHX(W;AFJtAkLT1ZSDS^zSaufy-H)ut8?xK-4fsjt|Q zUOqYu#x7%0xXRz(>00sFv&g$fk+mNzzBWF))_d|VxptWNc4pALC&X3ujs2TbZAy)C z{&wi7Tr}5bX^baNRO$%Csj3u*jpe9Ui+g95b%8ZcHhy4~?xJ^uoBF@0YOPh)q8Lh8u9=TV<+JAa4g?KddmJ1NH2s(#G<8;Fe)fE*V6fK043qD$^ngQ7VM>3V*V^(K`>g>)2^LC4XgENRLeHT9=7M4ASL&;h)mWNoSO`?Nqd z=esV+REJN;+mEC(wv?Qa>HOVCWp^{4gcudLY|1So4iXf|qq*uQLA!nHgqO?NrT(p9& zm2dTE72v7Y)eysCkh2Vk$0I?rbGuRW04|AP3`cT7*Ks(?SaLWL+)VI15nC)B9MFyl zJ`UM)DKxQXe`sn!IS0(`j$Y$6DhN4S$0!^45cLA}mw2b7E1UIq&~!=8h;7>Lu&<4_ zAL>HXi=D0jyX0CKY_`p8u|Z!MM;etFM9EGqc=5sRj)OpEYC3S+#PxTW<<;BSbVNz= zmvB@&`cw6MzX7eGw{^wjmI=dL*65?)c|2C!xnP(FPYW3|lI&#pyYP>y_toiva!dYc)+SehK&p!OkG(ptE%pK5t`cI3Qctiya}|X|4KPRs~aq zks$BLns-JIS}A|wZQUJ_r2;%Pw(pA_p*0pE*YU)HngWxa&i;j0)n4Ute`i|s46CDq z14@dzHsWIYMMWtZL5KZ5Iovg2A|x^|V6()|hC_4(vG4B@Hnw(9$w*ESCdbkB}*0E^Ls#hfm zL6q!#iwZkrMl5_$w*cDd!Hh@}Ih1g|^K%;ZH90?g9P%K-8a9v|&KFe?bdDtNI-W?U zOa%udhZ__DBZer0t^=Z8pc2VK&N5l;27TMXX?%4%n$Cth^?YWZTiz^A7wsO+7(Yk~ zI>V>Jjy0=AN#v+eg7=8@fZI##zdzuFxt!6ci!au&&S7-duXn*Shh8X0bSiM{4-1 zLlJnTe>~aVvruSort!$`7Dl7sd1cFaRrN5U>v`31bzBUs0HD%dJ=9wG{W6)8nm=NN3;an$V2eaYlld zX9kpyLrSSoqaa(r^VGhb^BxJNk6_9}Zb@SUbcPDXz**J?_3Qz|vFhlaql ze-LH~LmYi`sZRFi>5+#P{iV6|I3ML9!(SP^OV1297PR$O;=KFW!gazo~!SrV52p@+mVJkF8 z-+_ZbHb?$bqxsF>{atd&GMj=ZNj`zPjEW*GDM?@LVj^4|$6%!a8uXQevfHPjTq7ZN zjR|1$>!km#g7TB6g$x@>c9x?kf<9n+ST`?~YXtdIzyZm<5wY3MYH0>V;E6*bV_u*} zL42Q)$LrjZ>LhAG_*v{s@b5wu%UjHh=i!ad7|nxM5`vhT^kT>RY`teCPK>Ye)+Is|e&QO3<~@ZOf$R z-uQ6&C|^t+w&?;XqPlr4MXxSLtZ&xS!+@3jJ zPKOa)bMuPKj@A7$s3o%Ml3Eb{Es7%O{1_OA?CJW!-2%1=(E5I z^R)m)kX6XsKCl4HtsPq}EH4Q;TlWhed=vcwRZRi%4lr92dMx#iix~8+X<$3t!$715 zjE;wtBB1_Fm^5d7LRA8t@9ZBHDS=Yk{M7|a$pu~Q0#uAhB_W88RD!PKm^3>=x^XPg z#5-Ly-;?8ODgFuTm2k-%u)T%@6Uij?uS4cCIbd??K3BE9*v6b1U7at@kV7G&SdT+a z;ULAOfdkrog84|zh_g(Xk1tz6HniV8z#P7JM0|tKdh{<(7Z)gxfImIaeXD^q=h)4` z8eD@$Mn(yjNUfI(e$BlZ%IhseuU`AYVefZ6!>N6&m+Jl=v-W8IO^Vm*Z3D^X_e3vo_!HB8fVPR-@XV|wLT=%hM zmkg2>n{PPxb7~@6g;fc!1LH7KOGmvnMlx<~$AD428t;CaA(4qj8l6qqlmZ6UxMg%r z_#`{^QK%EZ%ioB?F!m=G!*ESORspj%K(F7|s0XcgIOTW9eH!_xI_dKtpq9A`$NKM$ zx$w7#;}410y^0t2I@OU|EI|YV-v1wQZy6R<+qMl;Qi8xBNDkd8jSeL(-5t{1H8dhp zN;eV;(g@Ps(%ne6Gz<*_-?qm4y07bg-sgFrKi{`~+crO#S!>pudBnc&=K*{dJ()Dn zQ3p9aK?ukusG2uQPQNMUnd_0fl0qD?T02F$#x@hU_3>Dt z_0U7hgDGYo8WS`8WV4$+=({p4?qguT1#zHa#00jZfW*OvC@621#;egWuq7MZtd)zb zs1g`;MQ2jOx)MUe!q6v6RD~<(JP%)%+(YWb9|!BrTvxEfy+j*tA+c)}+Tqu4@ncwe zy>)&janyZYf_ma{yET@X(O_2n!#bnIg`D4LD2ny&9zQZ`8yVOn_yAaOZKH3Ni?xSgw zASp#m!M}s0{hSZF?RoLvDE=C+k*&%x+-5hxS9nE>eSPEj4N})w-@dqrJ<@5E67SdK zY}R=;kQ(|B6}DJ^6y{C8rL~V@x%C#K*AQ{SCF{U)nCfb7!i#BRXg}QW{ze9dz2)@oI!1VF?tQh{5F?E16bs`V=-|%>#3$lA0@M_62x!t)?^tzU# z4IMH+!c^pzcy$Lic?GfIALeF>JUoqDgx7?l1A}EA2Ew5b= zng`Y+cIav#66Wj!shJ_~toVYHEw`+0X-#|_?#HVGy9dSRngow64k8$T9eiDCFGjp9 zw_AR0C+M$PiRCQ0VlAhcQ+XvOy?koy9CD3-5z}T1hji0h33(&7I#;@@WB8^9u+zAV z{g1DRIUU9`2?^WH<3gLb3N0mYCCoBgy*|H_pncUr@X&bWa^ig@ zg3gy7-#cVUp3W#1=VtEY_Hcq<*VxDsdn})>`_-N1mvHys9jPaUH3u^1OIL1c5;~mm zrPJkukezf7)6F1!i-bYV8UdzVUtzWh;P8wUKA3gfr z;QS0M>_=G2{`K_A*leJosKsa4pnXPeO{8Ap2$kzKxPNmXoZ}_`S4h`*;5TL*BM*o7 zZ|5otJbw?fP$gg5%QwDfOrs%fQ211evNSFVEl<$ySQ`++5%<5xZuA?GkSowVp-zGL z1}-k0#+2)xT7xJN@!=tYg=4U1_gTt9E3%nW`ArN*p8YJbr6H_3K>sqSH}g3;09@ld z=n}9UuF&(2<=Q=mcoV)I3n*3PMsj|n!+d7l)4gkXT@e8LS!$&c@CV^tvuQK%y_#7r z!#-4ql|*RS8^fIA%2lK0^xq^@(A$Jghe`V>&!z}4rVe)EZ`ZY8ALe$-T;MEdNf3V8K zd01xj0;eBrT;n{N4z?-v5hRJ9PdS)~BGaY}cR@oh7Dn@`C*=dN#zTj-L4xzRTJI(O zDolJwrgyhhTr~^Lu|a0I54SN6HcVqm#kD4t#I`w+TfGr`L6?bk64;5D8=59mr(mvh z+^l!Kt`)+Kn%{rVI^R$~F>3hSSLAVms}STI;X()X_I}D$F)qGh^+Ma;qEO z#>j5CI#b;0Nu{!W9YNs7p5cN!s_&0JywZWm28Lvr?%225k&7(vTxGQBZBTz~D z+RSfTPcN<&wo=56Q{02qmEiue{IIvsYKZmr<2}g|GR2VIM^mletcOSmXpyleKtv8I zPXl5|WB2XwBQc#kS|eiftd893`N=I~*N!W2DHZZVb*R#t-5c`V*u2e%^@DESWf*$m};7Z(XB!CvUBqqwe5HM>4Nu&~`m<$agFk@SR5) z%u3gN1y6E>5L*K&uddNLdIs=?Rich-gn!Irxkbj9TI}@G*BdJAeX zYKM0t8_2qi7h}POx zGg|B%`UHm1Jf(R>?v=?{L+?bP8BWh6BKg!4m=a-0d$<;Zf~@7fT3JyVTMY5r_K9c3 zRVMLhb3W8-NfX>Oe_?%tTDc}avlV4Bd6@k5&vEXBpu#YIopIh8Iqn~4$<_1USAXsF zE(_buBTPkZtXF*1eu243b<=~=AFOb%)+GT?w%!2AR?SWcN8SF*t0p!4cOjuUL!R7& ziUm@!zC?>Ph~Ut7UnX$4r(BKP?W4G$zLUlxTe{Yj`2GaWb7*&yV5~VAHqs({JTr{< zSf4h{h}B|ldZnCoBHXXW-Sr#rZ)NFP&+Ma{bhwK@P6tpxu60jtrpk(v=nq<*2#nZu z%lwawT1j*@mzFY}FNXEUrUuxBw}6{H z7n!ZJzEc0bn`5C_AOSDd0!NYV374xDDq@hsR8(nha`fG!EPe! zyNYWU(UuY^?Ak-2uJLB+slj(ly&w*qT{>>my9a&>5Q9a5Hf8b)R6b^{c&FdErka%B zD!LQUtQoA!W;evguL_m)p7tg^!hg8u|G9*!c2i+zebOr&;kZp6bi{+! z`JH^yyK%Foeoav6(9M;vG;Nh!qchHLPoT9|pgDJHY;ASF+lqW5Bn>T3U|t7E!9YI% zbMw5N&2`HY`}UO^r(#eNHKRhYb-g$hP4G|+b&chMqUXbqk0DE$)=kxwsnd-19@XCd zt$Zr!_5AqdZ4vKH#c$apF=7HULFJQAbIR1o{QcVl#L(>!R>U~5)^`PR&AR@#dL^WT zu22i#dI=2C#-ji&xf}_Bje*inWbSKe7n@!xn_?D^4cToQ^7&%YqeegWOHO}@{7u&E zi!9dueOXqe*vp?*E)Ippg1xg|>lVU@b28AGA-9cRHTeBsgcxZe4%x|%AX5gF@AA%1 z2a`#O$G`Fx`ABc&(BZfUe)=kA!CZy3DHe$vzyMgrJM{mOHs67w;y|12^x6k6RctRI zZUvfzLVN0}`IOkHCoUgbc2cIrpOvjwhS&}j)v?~ia=@N%t_C2#D007Zt)=Mtoe?_t zTJA`uLcTzJs=~%G6lLQ6(Hz4AcI&IZv;b6c{2QNcr`Pj((n|NAr<`02#D8dz|Mbi=?jgVH@*P3!Yz}v|V`0h8X*3{2e~XEN}ZX#FpxVDXQAdmQ&b^9C2t^x^_Y; zS^~3XFIiRDkG`FFwx5HYy9)Epu(N?XMkZBS>B^gGWyS!9;_;dbDmG|2pF!z1YG3}_gyi>fMN%AS<51aI-br#?+-PU)D zZV;S+&koK>_)%s`!TTYjnDo#XFEkFr&Jz~?v3x@U_o`xay~)T++9>O-)zgqYbzfKH zO)7tAOlm0Ek`7Hntz%|3aack9KX|eO^)d?rbymHF1$sFri0y_L-(LRfOnvfVyWvK1 zJt`&~42Ku=q5!*V0S^iX4_T9>eDk5?lN=fXOLWgwCK61A%w?m(q$kUMQBziidT8~u z+*jWr5bi@tbQ0<|#10v+YV;nT|7JLyS?S%pI&z31P5{I^pCe}`E#459^bnXyeRgSw z>>@R2peYvM$FDukBPN(IL@vxnPo4~Dt9n!iy=;w7lHZZJ$#S#GHItHf-2w1xrOnRv z6-n4@ad~8S%mZ=qFv?`-aM8Y2zsYY@MTEn}QGLJ7mUgWe#8VZcBMyA;uXd%OoQ7a| z!G2I4+eXS34}ux9 zGj#)KYS26Q7=V@{*JQ+|x0QBh3=IxzU~L%gW|sP5sKZ8{wuto%R5dS&%(AKWzM3E3 zPsL%M_#-Bd)ZH0oh(2|li`#fuOlOdiN&4kZf#9Ts;YQCNo<`kytFbawm+I@Xg~yg(E)W0`n+cw7I_HYcRm0cj9sV`A@i&c|{y`PJEB2qRNq0lp26!0wEyV8Tst;A3>V}bTs zA_E$ugF2UEj7jGTUEgyUqME7bCCAS@n-ao7ep&c&>-mw3%-Ug8`$yZNug2po)E;@p zY0x+tOLNV4t0G8Jy}=uN?pv=QSpM&7hIUCS9>nCp8yUCDfr>WJ@w&})r$Y^jsGs~B zF6{sn7vOzc8xngY<$D(jYpaa}W2Cdz$MKjNuF+ff zZMt-(O=+G#o5S$E(zeG+_I~{$YtV?iKJ~>n(mc8haGBZqaIi5}#&?MfZ#uC>&c&k4 zG)YiRoOJ(2pE9+j2i+SN+HdHgzss0-Sd7RVd5b<1k+piOEtXL^6CvbmwDa65f3Xtd zaa11I`c=Il_O8kG+=YnC6ysZK!wID3Ro_^f$b~}=J4*u26WyTyX8l5#pqBE{^qUc% zeeXZTaLiShiM?f2ib1^BTt1!kJdDt(!Oel`9&gG=yj7DR0zMsc~WbUe0f zcMC<3q{ch8iE$%?fLJa6ZDS+pp^$eG>RH7zF?P2~dKqHUB=)zU`ikP9*8Q0nI<0o4 ztiqtn`_q}@k?=|zV+y?jjS>WsWH!vyueh2nMRAk%Q$g|0;2vhrH&vyoBtKlt#M9K7 zp5m`ka~)@AjyYS?ATDQ2 zAPXMnYw){5Yf%wzZKHIW>A28GG~T9tnKU+1Vl2H1yN=U^1&7>YnE#EShE=9eCG`u@ z&#ENd5!PC!^rRl`7?hm2z)p~p2K35jP*mQqk(@N@-m}tM|0CnA)rOV%66Znr;-^Od z#iqMO1u6*0wTFbCxU+vL&D9Di@mT4nECt||MQ7FXYl^=s12(`GHI3xL|LN;fK=L_w<mqGU_3ka5!^l7cu@!CUMG!F&F-*!4&4WYCn#0|bwZ`dnG93!1)@!Ek|7O4!PW zU^~mwWQ!~l6c8n%9~@G>IiqD!D~u6&)-cT)4KYleDum1+9i{r6RwnAeo`YoEkiN0v zBz5FNv|r0}3vY_y=37;t?>E!6vwjiqI83F?dU~643;`dB5QXp}SkJ zeQdj(b-##f5L#Lgg>oh*L`eVjtuES?XSgUL529CI(2gFdfj5iP)YSVIy)XNbjW9u! zh&e!$Jdxhwfi(_` zcDT)w*pk1FWwmCmL5bWDqh0yc7b259O*+NgVxA;Q zCRmTa?*NuiJi=sD`%)8dT%N}svQic|)-E93*F;h6&o3}XI zWNwYm9<9_|F3C)|Km}W!Edfm^;9BnVInBmu_c_beRdZ0C-vEwhFw$t1Ihq1gyV(~} z_`#`wIY1qXj(H}XL-J#+4yvbKpub*yQ*AX*AR9*;v~ zC${`YqTyd2jWvm}rBhF+fpxW*Lk!(na?a~^(#-i@fp0t=!_E5QR*NwvMoScld2FWX z4WV@b9OwsW>XLoq1m&*tc~&r|bC_h6t?HvJKW+@RpB~|r{@y*6>fnw{kspW1YU@wK z{4BqF;N_sAtOM__upU9~SJ`};~ zuF{Eon!1YIFz+@|l?fZoHzb(k&>=DiNGqKylIrcO37F2Y$awGM=_x%GC!7X5?z?qd zJ?qQyavYo?`q`?+13eaUMiF|+({&kd@x`HY%XwPq$SbKoxGP$Y5{ZJYJ^!)esgU-rtZid|RAkEf(tQ zth7`bD}*Yjm#E@X1n}aaT^@KVfNSioNORs_ils*hIXNvWF4TGG95@$f7H6gv9{0uS zP3atYP4|oYBH>j!XTED{3u%-*Q5m^UJLpEI6-A1Y1x%OIVCH~od>}GDr!E*J(fTO( zcfIlbLV-#lYsIc_eRL8e#b-oZ+e;wN@n>*#sNax{c`TaXs{+0Gyx5b?FkOsF5sJcl z{;L!GPxaoNC1E#eyPd4qv^;O3n6!6b`IbKBK!Bom68dWRB3(erPh9a{Kar!g_p&E?7`2!!DhfdU2w9<$%gapedzjh zGuo`*78ep&c)Xt-<~cDKJcpBlce41+p8BTR#x54}>7^^kv8Fhhqb0paQ8f;Cez0@B zC;E8h-oWgn|Gin?edDdk(Qv@?TqEhTeg3E7(*Bsu>#|Ub5`Bv2&Py)E8)BIOVfiVe zi}+wB!)IYfnhTw&=^VG3D-nVg$`*hCK$=z*Iw}h$`+lNh0jP55{-VZ9!M!?a-gBaC z(zRfLHBu(LJU1zvJyJH=3Hi76*AfyEZERpDCqKE|dSPm482UzktPo5k{z{!yQ|3;u zVG_(3$8gw4V6vrCZ8jF>A~~1^^*UL5q;Q-!MEFwBV7yKj%s%S%oIei9MYPm_dDfetx2A(##}B4(Q~xU;3^XVSWOa25}8;Pm2MS z7o;|t;16c9ITXt_#TS+hw4zAeaH0Q)1LwhyC^Bu<7y`u*2=q4*=?r_MU9F=?0L2fv zMDYQwXHDS<3B^RWY{lOXpmi=g5N~(8cL`wdui0u2F3XdSo%o(|-?6OTj9LT>j-S~{ z{5#|$y3jBMvR9dntLf%J=;%*+erXaL;fGy0eDV1CERcW(@fn=Zu9MNSQe+y=9ZTI=5OHq!<%7!-D9?Ke3z zwuO=!ROjaY5_8+pOwp~I3K8VQ3IA5GyAEM8icFg!hCnK0mc=3oo<)KZ|DjXMVokMI zC9FlSe=?yc?%W@oQG*pJiA1|%g~EsvHEqJm0RXwP42b=~el?HfB;kU8P&VXOM0_}) zLkb)KWkc%hCMwrxMUlQq1j1Hcmku?+)j0>~%3!;mBqlgau!x%tnN?6Lr-YLS-2X?2 zaIFlsdz%;z|DqpWHe^=Ir<@0V?_(s0eK={J{a9HFJw{C6W8n9Ub1M63MUmLy!LU=k z;s|Cu0>g?NSPa zwSiv&JsVbnJDGdKVUjom45C+oFk^HJ3;3n**T5y2$tS&ovVka{LJ>CGp+Dnz|EtnS zM)Vm{0-6CQ7ap85vV~8Bfu}NI6pBn6u(HVVOSM(u*OK6TlKt{3u)}F_KO$@fk-r79 z{7+eM=kUAv6EC1#e21{vjypK`4!8tL1TMuCkjJFPBfx!i@M#sDExd)!cyMWh2e3>6 z*-_+S*>WbDv|QfW%f0#X<|hVM#RN~V zRVwr!5aQwCy@^k7mq{2%atrLGg`2KnbY<{R?N*(YKWRmg6vYBzD^G`dU&DL4j++e;P}KXL3j_(0|K$k7 zT<&oKTDr0M7vPDa@F6O9gn!K*{kV!Qv&Jkv^M^WIl81+(3`n0Y*KyK3%mvi@*ysVwAx6U z0GcI`(;Fn}C(SKai0w+5%$U|fs&A$(kA5)W0%Entt?VT)Fk%8L@w5L(%4tQBFsN`f z$fynJHu9)&fi1-8t-buNe7o|`_VbW1UdUb6ux7;Zm}F$d#E1zr!1KDg!tf3_K_1p` zM#MvlMa9SydDfSK%q`-Bv!m)G!NL!a?poxja0w`)5FlU7?&Se?Pok*0vvLRSn>0@^ z9aH|=WM;f{(SJxu`{L&4!xpGbl6vXqw37?0N02SSCF?!;@P!4&PzLXR*U+X1l}EKr zX0^0aOwgO0S+0+@$Z>T2ZgekfI8=N7Lo8B4pBqi5}BB_2SxIu{ZG8ZzdcVrBOH?0rAE1H6Igz~AKb~<$2>s3_&~b}B zhx;}GT*UrhWM0C^T^LH9{BIKOfIE!f_tD)|Sv{ZKZx64oio-GDLiPB|Xw3W78TCFh zw`2$yeC74&=D0HNNbZeg;W%7?MSg+`xH#eD$^S;(9{wcSp8&R+Y9U=Bxqq{<-xKk} zmD6*YXQ9biVaR0CaU8Q#%0FVGPmPk5^=W*-6{a3v$tz`JgeKPwiDYo_B#ap&2+{L+ zy*6v5ZHXKm8~R>#lX*2bx>a&`;d=6jA)2CFi=s8SIGN*%TYZGpSPhFXDKC-zi;FgC zqxRbk1kR9oY%Wy89F}37UCvo6qxXxin=lEPqNr$u#jamZ zu^wt|^>@Nb8^62EgR6=R2<&&YGou9`tq%qZCJ_Cxc>g+~z(oZr7Ys@|?_XbPrnFmf zMLkH@?95pSovqtTNF$Fi9;*1tl02Lv7Oy#5V@}R@Bvd@UJ6}!wN?aWG9-=?=s<+}R z=?uGglDzrgemE74aCf+AX!**>@^L%E`sjB(BeI2B{9x}IvBX{X(Q<3jPcQe1{R1kG zjIjb9T;$~q3o^^PaXPL*>!kg;~=5q&)EC6{ZZC=G6~QUngw zp0cQ*2C&b)d&XgyZQkti8X>KEH;h%M(sXJZT1Wac@WQ~`^vrVE^lsPu%4p~T=$Rt=}4h4!me}M1kTao zPY*-FAa-*;PtJ=8HI4QolbJ(Dq;c9m(WRekN_Wz1zL`80o~>9tn!#jCFZHCj3}y|8 zYi}5Q-<$a%?(0CYOO4HpMtE0i`t-`(XO3jXzGVMc6rBt8QVXGT$CtJC9UqwRozD9u z--U!wZOp0h&Kk9@=cdKRa8+B)18|t^kL7X;VqtGb<6OvOnJUrc(XwBuZv}(I#e4ay zEgD{YYOy3!jUrwAjAgPw-$$5w)3^wXI2P17kQ9uFF}S~jmnosmrU^PecH;S_gLHX_ z+LBbXuQgk@#{*H#ud$hn>+QT(^E}opt2u6URbx95bUve;wi7}X{Mg5HY8)4eC@!pw zd{pq+EHy$Vjur#yx1;Iic)dWmWoVjvjNg;epYELuMi04PvQC8F^RhfErc-q3kAKI} zniYYWXq&BZhiM}i+p98dw?s5vOY_@7UjtTd*&}jQ$%J0^mlnWeu-pn(?tP8qd%rq5 zv#}al=o%yJ_r7=SD@>-;f>lDI=Altc_QM7THx6u`EVZy#YxK+X7f4EYB+QteQ3`%q zhI^CBamVF-MT75td4DuF&U+I}3)&cCBrY%T&sKbwjlp18qG9G++V;z?H~IpeB&IzN zI10K|N->aLsr3^hzXIA^=-#jmhqyYFlTB~;@#bbHyF9DK7Sbi+N$l!fzK&?M z2i1`X*1k$=oQk7Y8&Vgqe^=W@diyUtqcHCKom$4Gm&WH)ABUHU7^^T{58DV%*79&~ zDjf_SI4IqDZ=Iu|GQ0}s>9;@LiA9qgxjT|P5e^)y}Z|D=XdQ&6RO{i%jz9e>6P=IhaoizMMc=- z)Up+!CY$2``pc*ap8lr$K}p1mv>vvhm0scbyAko$!?BZJ-)^==L{V_YVlkK7x_ z_ZgFFo;v9_9E#bGsgI{Wo26nBRugWv%j+kg{pB5%!Lw+%_BD3x&#G@2yjHIN9{#cp7Vqaa#AZCPSPnlBsqFgMYK< zKkJ*hiQFEcgu*tgri<|3H(h@J>GT`Iilr8Ck4mb^A+D^9F#dHQThWl@zJ0rQf9GqI zNyhQHv_QG%OR$K4t_5;h&3@p$_>z`frUZoWo|pXaPqAl;=Cy*NRh%Cdmn~%_8puqkGwxBP)A|ho0Oz#?FKKYmh$X4q!8)}+3Hgz6 zbQl%T82emM5JGDu?S|<1UT;k%#Xkb%_0%w>m(BWY(1WD!5tJf4+0?ObzyS@`|0Xcaj zTH{(2MjVfM9#$ft2VHWMyP&)}3o>~Or>!7v9N!GUX#Pj~FY^?M8W&`uSM;=66gVI9 z#ZB4bH4^QLFK|$1Ca(k!BPMVU*XwMfu*i-iTKz;b4S={aYTxn#g{hbj{~VA48dU!H z50Ky{K?v}1Cjeg5imd}DokpquGtDafrFnNqNVP38XT73Q>hY74l@LhJ@v8j+dWB4!FWcBML1i z)}@yR-du*r7RO= z15xy&;JAlNMkWD0BB&f25jZNyr2+3h1HAu2=h8_bC^Bt9K>zDbwB3MfoN%Jj*h1?K z5AZe&Ku?6=XvzOOKuXodXOkd(!fn@;4&EV4c8ZrJYahVV{9NA(Sz8_lO!;Qw!M!EQ zAOnB~rY!3b#RK``8dWwOATF*-F*<=60ZgWlYyHUm;^}}NS-h; z4&^YI@qG`k(F0#hz}eaUV26xwj93JWIzB`;X!$$EEgo=zCN0Iiu$SuTb9jv!cvi9q zhb0=^>FFhqXjgusl*FQW^7EU?WMDctIK&=rEZaQe2>iy&l6`Q=&GQ*v<_QTt`%-XD zxRm=j6^IgXhTb?FlKVOn%aQ+Ec$(xXjWy^}yFKHiF*vqT%j!=t7=XhDjRJTTEL+dg zZ0=HHJ1dPAkq~#JRMh>B%7t=1-W)9j9A2XaT(eXYPpe za7Z(y+~x7`(%HPyOdGaJDETf{q#UOShR+viLAi!64SwjR^I;@hBgjLbpQLH;}h6BrKG`zucW+GwP9gl=;^s7;$MC+ z;k-~b0|CE=piPZ)x^xgOQnUYKYu%EdVNSf!6xMLSX@IFUJO3CE@jk)7AC-{%cl-WX zDGmWcTe=Md^(r$rNEluZUasA5=UaY!l`C$VE1ODY#Ht(3K=t{TS}o>o0%I6Be#?sR z@Ac6382}KA>b14udm<{34i-aaK;pYtE$Fa})1Ci#I$OVm?COgvcdx{G8xSA-8?Xc)m!1Xi;sCySj zC;JwE#-R{Dxx=VwlojhQ2?B7qFUfK7W8(Zpw#Gq`m0Et9!;X|f*xzW&eq`WRShm&< z6OMPR*$em#DFo)TId@ihaJ+%z3Y@`z);#__&f6*H5}2iH$Ew^6Z;lF3eeAzomVY)= zS3fcXr{9D_Z`KgTQ0uh@H_1O+^`}M?K0Y-f$jKf_7pDAOl)YN)#(PBwxIZsPZ#_~D z${9)vKN%b-48Pyc8>9vR=1?SEm@(4E;nsU!9ib^uUZ_$QT?*+YRXchr96m`=0JaHd zVdCz;yLm_XHy^JSY zgYwGYSq`|KTmWtqoSQ!i#v)({@FKbd7*t-`Y=wA!etof9Xm7E^YPKuqE{dXXgHWni*FFeq8Se9d0XLwW&g5 zX!ME+ha;(lbyic^dAA%y3LLU7`VtJuS4n>Jbd|WMfCoFeWZ<(7Kk6rz9MvTOn6owz zR3@J5QMZRRa3E(zQUXPSb}7A@%+m43)T{CZV>$=gQdTn(A`XT1TX@x}|H;sJH99Ly zGN$3E%^bCj4e*y6OB~5SkLA-zmfakKCT5O|*LDN`b8r72ChoW3z5?qk6x|jl>s1A< zY`L#`s|*x30{itdsU!ZP>^-H*|K$NCWQ!E0Tld(@d$p#Cq2o~BW6AA2D_j8ii78@>c&*ccD${G^C zn>;xG{%&4o@#atUNnq2HMIdape@=@8DqvC~?xAZ_;}pg*bZ%#)eeoK$i_K>X2?;IM zLC0vBaa^r@WP8H8U;n>+Jua9u`w+^Q%)!8wuTaXBn4=S|-w@Tj{jF``{96Lr-p@#- zf6)p5#dh$@S_zB)Z7Q!&SQF8jZRDY&k+c(VVBG!>0aWdSVG7?c#3-zZp+zCE6=xa7 zD5K^=)nq#P4BG_hpXyBI>ywhzpMQQ5g>r_1{`*~?)EjJ<1Rh(%{@r4ZlXUblO5PfB zJg;YP%*uo1?@5uZJ#2Sne!yo3@s^5b`JatTQg5e{-_KQ8hUo7mi}bhC)fSK33N%rX z?`uU5e17Sb6{Up}J-4m@&&2?gbNJgqv&Nzcrk&55lR96GV?R0wHMDnnIKls5&?I)( zY!amJRc0>acJG|`e|tnpVki%jn(Y-wkCX#R!^^Oq zN%xn>76-4bj7qU?IFOprEe=ZUr zQu^DyAZ)f{$$WtGVnC?~QK-`~``{YD5yAT1!BfpcJ@ zNZ!Dqez90<_{z@!4wyiZzk313b%hcj-^P_$=>f}3`wn4~it6l&^^a2t2Qqy4PS*z* zwnrqX8nkdH_g|%15Jnun_WnNym0AF1u2@K$1Rh;HLd>k`!k(!4r6nJaBGdK=Lm8}J zKBfa7C-|t<>E~O90Hi#S3Xp!2PlS&R@XIh`rMp;jz^eknK{N*lVFqw*kQN>w&uI|B z36(VlQK&Hve2Z_xLuY{i4ohs=Kol%0Q79)0H#eMHm4Hj4Xl>BUwg7@wJP@`5vF}R* z#zRye?mAn@|ME4#>_n+>KQ#*!_1$d$_LA|jG!pHK0KjEWXtFW^gQG16EQj)%BMG0w zACb=l>g-vb9L>Ie+`@2jykzh$P!U3?7;`{uA0@mamgm5qO) zJ5|bl6jfdP{4V`Fqv2<2sXLyzH8%_2mxS{(B~rC-j(~rGSSA#3c%`yGSA}O81#Z7< z6NUKPpjeOROV^MIs#K^FOgfB#cDT)_2QTO18Dc8H^Nncy>;|I8cW7Z@UO|&P<5bd1 zzKlBcm^Qq&K|DL-ws~}ZW_v}60#GLrB!{K0_?1wZOfI^nCO_pWedwh19LZAssYD+B z?-j(BU(2NJIEU-;*4*m;;lzK+_`q5F=fD7mqFnN{4kSO{tc-SAAD2wjjH8u}VK=3( zt8ZLyU!>$kPofNPoT}vic|jht0wHG$y4qNst}agb-AAO~>a{B3pUOogJDywj3>UYL z!}1=<%W}cXtnV?$<98@Ah#F__*>F#jk^ie|DFx17@gq5j=gv-? z{yrp&0F1E#naA+W5#I+-X1M#fvCh#4-Rf|Uacz{td;Dw+dr6WT{)w-1UzhV zGpQD|P>_J$uH0VY;^BS1U!<|u#UeVz=$|Sfx77tXZkC9Y?~bH;fe9~QsXW#RP7vsC z^k^_7%s2GK6Os=W2945rGg75(}LB(h4xE)&gP;}8J@F2IP;4I-v+b= zm5X)07^Z0FofAFLtDEu22RGOfbGy7T_BV|F znK5vb9N592@jVCH#gFxs(Br`*@`D>3iR-j#n(L7Fkj_Kwz3(Q|6m@$6Xx7|jU0Rh} z1wkez?>JD;f_kHb27+bw^Zk6N?QgJ%_&q zoNs9s(Cl)7H9eRSeD~1uG?!4w`*UYttzl=g>z%Ir!;RcjkGW>H!jV6tdJ)e;Jgthe zBxTg$Qiw+7LtFLZ?L_r}->o~nMkQCa)1}GG8rw4E4 z!qeX+33=z-1&YPCmfhR(5h|n?Wgc9vGK7!us%IvIN{EkqfXA+XWGPb_2a;X_!n;!j z*j=!v>|c_Sauw70hR+D^9Wkc|@9?d;&Dt(cle`WeeBL6-aS`j;L-3etrCtW)j=^-(!-X6&^HSXk(VTHO>Ziu0d zU|5UTT7p}AEYW>&z@iV&v$8EFw#EeRgb!`)^qODG&W5(ifAma^wiadQ#?SH3{9v-l zERnZ)v>p0fsFC_?j(gH!4AW2eN_oh*i%J3X~j35k|8-eqz2^;j3@8f?g-IL)jD{V@_jnp$5S=SRPU!i^!Jo8T zEsp~fO4c}y0Hf>l8gOIcW<4#wTRcg3_oTyFkf80)pN|JK$$3fm#V6PokEd3G>YQLG zOX-|=UyIe%M)n^pAJYfCkn&TxvlvDiy2GL-t!QZy4Ue;dN>6e&DY#ZkZ0BzVm>z9? zoGrHs(ha>ojo|H#x!!DWy|!tx-k~O&z2R^$YRyBHuaL-f+TpUXmR+Kj%=vmf5qh+K z9ChIQ4f?RTKE_;HV7q3nEKs4QXLfSYjT4zo+s~{4O0zj2d5=?Z*74 zdpoc3d!ApE-WMH;uA)AbaXXrhq{ z$KJ4pDBxZI@oTYWr_VZrjm8jtYdY#apX|a12__s1Q4@I8BpFJ;O^LZv;k|e6L@gF8 zD=AGzY{BvxCVv^hHC40@o13VEZemI)rGc&=X-@A9j7L@Q+N>@*uj5Y=EFUd^JstG5 zkgp$VJ-1Kzrfw;EL=?KQW>_)r9MD${Y-OZP41Y?W5bXw7u^-y=1FodvK~7u-GGHTXt?RGEy|{Q+kx`H&tyL zdLel9!li11MOoC%_}7F`+)!O0QV3?>GKjAIr3R8Out}JC_okI>L*tK~ z&PK&{=ITCU&{dNzGEkIh(^nr_o1wBF-TEg(ZA(oBUSNaiYo}Wo4)kBv2GHa*x<5D2 zm^>=7ElT;OZ)CqfsbRasyzF2911s>HoKxr^d^L~s^`_0#*Q2_pJ^nA+yycHFU3Qa= zayJOf$6rkjAIxuT3;C!nSLDlI?4eFv23c%5{tVeKovX~XSZw^NUSHv|(I$57b*(Ds zzY%8iEJaB~n{Hls-t3zZMZ(@v)(Zq3G^9PsE33&7S+A6n4H=!1@xUx!Gaj^!X{D@Tby%bVB-1 zPuUG${S;Z^IM{pdF25(Hcbgz0?D1t|FyrNc+l{;Ea`)~7|5i2s#8USQ3>@#~MUpB0 zHxib0#4r4kmRSV{wuTaC@)h_TW29S-Rdq?FJa(*F2CckyAZA`^S?|H;6UMnlhQ6KR zS%eCkZ7t2?)b)+a2(!v$)hsFxnnMbnc{gl&eaHqrxfyzvPHE?pjHjF#EzQV3t9ahX z452Ef?Osl8J=FNwcR%{|?CW-Ni>(FrxIzyI%WkIs1A&al78S|)V_wLwa}Rsvkq;DO z9D?TGn2)3dosC7f59t$}5pO7$>X*pr)jfUImM=%o>%}qG@TOT}EvrxYP@Mlz&dbSE!wMe>o%U^8OK_<9a*()ckv1Na z#-zXID~1{0c0t>IJciaNm?JYD_&4}{ck8vsd;X*sA-{6lk>%-pDg0m3PVoe-eYfmM zEyG{6L;fjj+^uNkNrsKqk}f=YX}8e!iKQ~MnhK|y+Z63=Ri|<1*A^}0SBanj=`?ri z5yUHHVeQnt9U^-4lL>iV*tnr{(C{>TPU$xFvZXcrdif`|406m-9ycr(Yd5R?X!S+) zp!iOXl2G~%E5svDGXQ!=!tFmcj-^&_| zvq%wZ+xl6Fjx$zZRIu7^V`PD|_JStXpUP&5PKTrhIvT}yaHKg}(NMedv@9p?WqRv~ zeswoTFRI^Yt4CEw0V+8*WYs~=D3iufK8tzks7|gSiiqHF6>{g(5u86Dy^beJnf@3T ztnZI(cjHtI5it|h6?r%tnmWWa*j;>Mk@&r%b8a~xIdpHq!ec5ZpP;Ofs#9b4|LM-k z-=SdJ_RJUz(o7^}8G|C(N|I${Y>~0_q_QtX)+|}aPKMEnEQJV9$iD9~3`Mq(C3};7 zA0m6c89a4($@h+zZ)OP zHO+=WBXui+3EhW}c1(KGr5>%71Sb~i=tfT$c5N+QMLQF^6MX$=`v%z^4LFud?xT4o zEJWqpr`Z&gcyoKI&3ToS4w!qciFht)POQbeU)Sm989O)p37>VYUlMH={t`iWevfw~ zY3-Pk-)3}e;`lq`QZmKZhR!2jlIdg1^B70R_Y6&z6xFkfoDPKUf;;-*tns3Uf87ZG zO1KoNP96~N_|9QLvh4N@7g^@Y97`=z=riC}x-!=6;Wt02M|V7Spyr|JL105u<;pld zr@y*sf4MW^+k-E7$+kO#rlP8uU5HFMqZO+cfA(xsuElv9DK!}Wz29Xtl!8k8Y z47O_jxZ%!cN2#EngzB0FPpP>bd#H)uADxXI^)6|wT|{>|V*$DRZuo=|GAQIYVR4ML z<21cvDcYv?1yAyC%LfO`AFrNjt$Q-r!R;Erj(?`>o8B9FY~Ym=AYYf$+g;tbao{Czy#o)+K!wvoh< zuJUJ{X3$;o`Rd$h?qqX=%6U#53QU2U|yro@+M{mjCBjFVTc{9z^*cRc<2oUt%#Ijo zoBuhf|20Wsb2Um|SZ_Csk@xvtj{?4Q|9*6Rwl1Y9C#8`jq*3=`q!@PEU#Y=0B%#LQO zsWat)A)5aR9*RGG(P`k$8!7*Y1hcv7LCyzNypeozVq>d1$?<(#RPPqfqz4{beR19a zw)ff=0=?MK9&zK30{CJchdtA5d*uy7o0DzkiUT-rmX1r?R-~6-AqQ4b8`nk6ciqnX zGG4gQSf6wyRNt5EA`|4C+0wtf_DIA{r+mTaPcE+Zz1?8ag{y?_i)lW#A5 zFEJY4nwPxVr?sw--|r-%SMM+7r2gX)r`NJ}EZ`{Pl_?_(QKbVKg{XId+seD;TD+Rv zb`=@@F5<7`zADD>W9JU<<VSBF^x-p!r+j20~ny`is_I$`IDK%4^lD?@) zUN0{QKX_QUVuSlA(fYl_aiNASL}qdE34=zmPe)ExdEphy;mSR8vBSTc1RfRl&6pxE z!8m_70!bwG-R!P^F=x`Av|5L+IdK1;On8oT0>UD>Fl~>G@(8@|mT$E>Z=?qkU z$$QP3VPK)=*uXH0ued{&m7YhO)5MuP57vk;s*H&RMt5=sqpn#A!}>@WA&%o2iz95`t}#Qb($hl^f^_^R*YEre35hM&pAyAX&!Um^c*HeWSfGno zCm9V!>wVrQUm%j+#JH=#Q8>&DjL1j{t)4Gqc~O6+pJcyWe|u&jce*)R(8k<9;9EXY zt0vWGmt-M|)8@XUT@keA;w~0YC1UuYXsRQ*9eZ+7t9>BzGIh@Qn7U+b$Er=G-*#W% zcnzh@i`-MWF4kkuKMBVhn`g!9@yXWgnaZH;w*NBM z&hmzRPcegh_po=IrLYxxgSh*9yLw!))0cT6%R)o_0k9)v+Gt*_uI@7SHLLPJXU3MER zey+E78n8}C3V4l&P(rV1ZtdF&$^~v0ofI*;B~hE6XKEpOV_|f#S!Q1@+u{RNnez$W z`Q~7sLhh%LaQ)a|$eefO!?=0!8}go8 z?i{BC?_p}vOZB41jcZLB+LnDfk)8&nGrjLsTeJtmx@kpoVF~u+9{PRe{rsr8+h2Sw z7BWlyJb5lQi{0h4;~uQ6d}u_LaDLWoL`@qlA8grBvZ<|bmYH3gnA~rs^Dud8&m?;W zhOicdQs}7s%U*+@3OANByX+5JhLqlGK4BA*ojdtX9(S8hjV_FF5n|yqd_L(eS#lSb zF?n|5MP4B5dDFV8`OAS;x14^>V7qO$F6NSVmOIY)tiBlRs9_!s(BQM#RL)=5>DS?Y z_WD9HwVO-A{GE*YpRGm7PP+1IxJA0cYt7t#jbbD7Ze+z7bH}+L zdY+tEX7Wwp*X+{)92qwMT5z)}?XFYyU|q&e4z+g3hmje%>&F@`85z_i_Wbo#i);1+ zB1W&cWJ*|(x(lG=bT>5X&%1I)itd}A?Lki5`h3oo_J?mP8eIF*+vY}T>D3mV{t@G~ zUP<8!%gp6IV=Y3hDY9OD{>eR;zwS>=yh_IE;fnC4F=X1FBCFBs(Nliyyv zr&=p)X7cM4d*8;pY+O=-aJoLs(UPgmFm23J?dK|yq|M2+UHaq|;>5W8Bd&%3L9VuAYRTzh%* z^8K^b4bLuLuhk)v>{#6WL74vew^4p7|9K%Vw~z*X#a9pZW`gjujZGcf(Uvj?>wnYD zd5Xv64Rj!84A;JEv{cSPRu25OTBGbsr*hf`oBuweLWp2d5LSmk;rB1#2pBBfv*`r- z@%{V4oCrhxVTad+Su-CKWrD{G>|MSl3$oa1U$N5JeV%-tBgfo!i+yl^EHP8&K+LAt z8AIhC`1uwIg`*-qLZ~4$RG*m!0rB)nvZ=t2se8sPbI-L_tQl@CekZvZ7)0TJaYik8 zk3abQw#sv))-mgtU)9Hx=S?-Qcc#3)=Z_Z~(IG@+$jquhpzxo9|H3u7V;KjWD)UU{a)-P5qQ@xMrGwxKT*zDfB>RdD49HEO$5Q!pbC)?YM zJe|Exh{(D&dmZ?DWVd-ncTx<2iK4-XB%jl#qSVJH%#Myd4MAWvo zoIwNxcIgBL+e}P|!30BKk2t=2(~6c9Ts5^gLFgSB>Cm|G@4P z3_;Kdru`^?+G7rkCc}t~-UDHG`GX;_6C4<dk~gkKREVheBWoHPzI;HzFmwzw%%8&Yq~A%*^SL4g1Hf_7lR^Eh`_T{N}7~<@fgY z$kJ2!FXZr?gu+p-2@q=I1Nt;MfQv}4yYH*zfbDDN@f>+Pf8yHiPSe^nze5?%X|OdC zLTywmk5I=C1bj5BSl(@Xo8wk3<%XYTE|wLa#X&-3nGIQ*`>WTC6E+BLea0$;2rh9( z@SdZBID)I5c>2&QzT-TxDFM=f8$8@l_|LONlY5H2 zfz5_H(|z+Fyo`F=1+mT9t#3~O^=3e*{PDwUa)WCYdN-EML#@Ats6x42eToH2Z1!Vr zVO&zSrK3s(f^oOu(z3@ArCOeQb{oZC7p{zK^T`WkL#Ma3Hy%-mK%qzfBH2~k|B>e- zWi`|$rq`2wPw~uHv5s_7WAO{Zd=WTb-Pw-1gXbxh7xypYvo42HX6h z;VlEelZC@JHC4OMPE{niXx8p~a$ik49NYYJeeLXx2zP_?`o`-Ao2gN6rBuBR>Xh0S zb4*v5eJZRJcKr8(c+7&_O2Y%mNhPice@_}J{UmIMxzRl$zaN(zAx{V@p=GV>r45q; zAI%+F{`$bksK`gJsl(WGPxnW=-R}Y!UTcZVGkSJ) zc>Twmb)AK@w6t!{iE?`*x_%N3)n?XS4|JzNufl}nGzknRn6LFaUD0=U z3)SUo@v_`4O_H&Dq`h;Sf!Q~IyM1{r?5#}~>XM8=rj3>_J%hjygxi=mG*`3g-d>q` zq3VT_`)(l-OVn{LXWrX=|e8>yf&oNRa)pisuv;a}<7VQ*lnCW*-5Ci$N)LTQC>`6U7Q&qBOO} zvrgr_QlIYOoQOSW3A!X8CsD7-Ad@f3fqxw}Flq5h=?}N>RUwM%Ll+6N*|d@7GbMo| z$I0x2dAF5QR)?N=qVJm=?+VAMFr(0HC3&VNK_-&4OnMb-CN8ZD}IVG=&L;W-EAaiUKhI zYLLK+aep@wD%>`_>%aD5B9djMQ~Z~2MT~NxSWN!JDoa8<4JCC(2?v3~O~r8pS5F^1 z9vA|HIul|^QEgiY)|zyg^xSDeO-Rf{P~rDZp?TdF z?UIRHjrb5-F&%JU1WF0bYE!oY0=9)U5=l2BM_o5OM&6;&51)L8Lg6S{EQH#qKb>9- z^m!RtOG|y$6jn3;nz{A`%~H5|0-SVVLZR#KNE!n*$+8u(AYUHmYK}x8l`+LE;1+%; zC6x43T_5btg6UG4GOFD)S9s*6rV=KmHdbwUlM`t_B3|%;{9M3+!B$mBOCJNF{1#V4 zO(TT(_x_Au%<$bQxrNayC}Y_7Un1s>)awI*3hO2_)!ZYJQnSCG0BDI2Y9srD^k|Sd z3BaM82ly|kHk)U?U*bC)dKH4Mowe;siSw^^zwxQ}Q-)gOzT48^H>veaj`cA;#!v^; zQEvdsC>9hf?QFaAzys3;3GAF!uf9V0#j%(a^E$2iy)|5T%$R9?o(Ea@%l_e)%cok` zF!eYT`t9Iz3lIy=Co$Nn;tOR=z>%V0GXmj91^4@D@j-m05zB+GX@4iA9IT#u+E#U3 z$C?^0F6G@RG;){ipJ``lN@Ap_)XHhGgLVwQZ^J2x`KcAruTk%>`!zDX7 z>1*>Jm0#$s_^s2sF~qs##X$)bKcN6BYYH0%OUN;m2C3Xt$dVHse|$;otK^~PUFRWZ zzsEIqL^UJ*UJ*GmowASgpeYNbs8Zr$SpY{;%-_RE59k!|N79Ywt?(x@nh1CSd9#5! zTa+xGt86oBbZk3fzhfm&oxMJ!19O$VD1IXPk5 zu0Q;KNZuH|`Y{n8G2`cwNM-$EK>&vdN(mKiQxiOagM`rFOO#OEey$F$-K|oowd5-$ z&b0@5+P+wd>cyNT&=e3ff@wdJjeBl^rldI7jL@2NStGusD{hi4+c2l9tibc{Sh3qU z-%j(hwZHi)oRHMU_y}HrO^*qM#@>;0R>O112gKR;Von1L#`8v>(}SJ%#{!0`6>6ldAWqn76rmd08Uu)jw~1Pf_i9CK>-F zdFAyOV`LkUa~Q%nfm!`s_mzG$3y(Iq^J)^_8g8t zyIpN323|nBK>`b}?P?lQI*d)om1$k?9C_@3_~*#AE~k{GY;GwX#=d)re!Z#S^`((r zIMf+I<+r=wr3PZ1A4j06CD@$>nKdQYjDT33OE^rVp~Q_|0{WXZf@vr|@2;m{eLI+j z(s$Mk2MsI8!uRm)VIXtO0x7W~fZU={7_6E~v=vxOfUnc*aMGaDr$s=hje=q62|#eN zFe1ryV_O^sUhf=^@WUJkvLpmT<>x+`fCsqlV{n8Bs8kmC;HNl_5aCTP3qm65G>k}U z{vc>@l=DS!gdYr`K%gRs5Gp_0Qyw}X^J?%lHGR4dFs}heAkiWxJAqNk2@IC_cZ&Ei13D;0^;nb~Jy^RyDXMj%4Ld;Z%_t1^>~-F1W)QV_D247S0>HXa6yV~gyNC`<)Cn!x=0UUv*nPw*h484R<`FRur0ihU6CKS4g6Xb@67#yLM zL4Ae|*guUxq3;KGh=30WEGYC(GiX;NAUYgE<-fGhdIvCS#Ee2$DIbNe06&fpK@?~K zu9!;T2(CUMc4#1x6HrR12DAEQVAK_kKvL({OyJ-MKA6_<5vbc5_$YH;>NS{PnsLE2 z6rHs$;Ir8zn1)(qb`1nRUILlvFO32i$V|7upz;{NM=?Pus%KBiF@U?=pp?*d7PVi% zd?W&e#@;%g_yht&5Cw4rghu29s0Ii_7?E*>9+>1tpwK1C(ba0;9|jnabQW^4KV>y!(lRE zMA92KcPX$=hY?8;Zf+Nka4SG5p~H30kH(;cq4pBe%KL3b2xM?}C8$dzn*@uN5Z zazNo4`e8&ygV&_b|F!W48cGTEb5O?rdk@&?S`MK$_D`oh`j0UP3{eHrc8YN~+mRRl z`t#o%+kdWJIf21eWt}R6|9cNGYXE+VG(&Td{vW~%Ib>}2D-c)m+Gd78AdnmCx@rX~ HmXH4j(PSY6 literal 0 HcmV?d00001 diff --git a/Documentation/px4_hil/SITL_Diagram_QGC.png b/Documentation/px4_hil/SITL_Diagram_QGC.png new file mode 100644 index 0000000000000000000000000000000000000000..ab967ada62bf3278f320684df49449de2cf5573a GIT binary patch literal 75957 zcmeFZWmsF=x;6|HN`TS?UqynuwYUUnao6HfTBJa63r>rKLUDJ8;w@Ud1X|qP3GM`U z_^x!Fz0Z2jD}TQ~`wv$#=Nw~>@r=jsJA77GmBYs+$3;U!!&i`()l1Vlr7WCVDO zhKBa+cU%-28V#C)^h<51$=(81vi8J7*AaJvYmyQ@YlJI1jeMvpEIuqK`(9=*>l0^K z0D$F_D?1|{n|z?Fhlj$&eb!;SX#d81N=8P;jPKIsm6(M2P>ajKZu-Pc3X$MTd3pIz zN16aM48XttaA6XHz+3eHeE~r8MlJyPM(#7uKd;dQVT+*uV;EqXkOngSHW&2IYasB} zqkj&1qZELA6aM+tKd(tbvw{D9Ie;c0fD;`a$jO9D`_EM&K8pYID@GkA09cpI67W^dAZRM~D7pjsLN-fBKyN_@V!J>VKO?0NQ^-$A5zBzud!r(vSaN6SPY= zD9h}QrvU&R6C_C!iTfez+L*G1*B%cY6BOEn5d`}&S2zC*13uh@Oi-vUMi2}-F+b0N0iXfhUwVP2}Np0o`59L&@mx|G!SU#Zv|8uY6<{>Z*!l0#YBzp#{>Y6ipm?bFhQZ-s8viA z&J+A^t6&8c!8^tsV12irSLvG(L7xc~ddG_yJ=$Mst`~c0ZWnvSJ~w+MH?xWj>}^^Q z>Df1|&)@W>mj5-YCWBk=d8T-^GT|4?;`G+jYVEHc!yoeT-B~T8x^)%$oHHjIaZ!G$ z%1P2_=$I?DXb4&pK?%VqGz@eu5OarAn07IP(m7>L>c(UQh4aCA=4gt45(vuz&PT8 zqzn5Qnb@7t5I-Ro)NmG0mC$a|vnBr&%bW!4x@XXLS&O&Tt?2&w)NXC1p?Y*bqCVd% zP*$UEeblq9`+^!6OCCY4v_M*J{-&!g2-|=6ZXEj~ltB_2Di<+OP8WcN@qz`!-4R&f zzhzQXXy)s^_7|&q8B}QHNJ>0u$x1Z|5i!`@xz4O8TO!3; zv49G>ziIPKo?7jD=E9&=CkN;MSn(NJZ)hd#hYX0XFfbP2GG*H4vE;!k)+$n=u!!@$ z@9nFy7+~|<53ekM28X@%1&<}Z{a$o30_b!>hf6^Y(dNZb0%QCLeFpyRdsNJkBBoWf z-p`n&nh9oIkMZ?hzXKanit@EwU+HS*zAa$crW#8i7pFJIZno|v1UHWxHax)qKsh9# z$mB>O0+bb|W3H$`e|TI}%#@r}HnfFuB|mNOH}$%6_!jS9rISy!vT?R+8T4vMDV`;! z`Qc8eC!tL;f8DJy_MjpkuSDY7I*dUrB^LR!k^%#88H|Q#=T7~ohzj%Q+#u$Tz*3zN znry?GNxK=5-rRTM*OG|?Syt|^MZ8yn^!4uUlc^bl{2;GA|#bE1l z!iNYXX$r9bz!#kTX9SXHA1`VDSl)Q?dQ5u5tJ*i8f^Y^X3QveRI9~4xMwgkH(2>o_OEj_ayVD%{XmW>zXD$60K7Tbf?On2ycO(MHZZF^j=W=v+!qm>NMUiQRMik zLPM$Bl;;^S?omS7oXoLDxwK_KXAcrIqqMRo5UwgQ>LF&-<|T9Q&@HH0^FdhHX)C*V zp&)O*#d~V6Rr{NCk9yMX#Fq7lhjjc`J^fFySi#pb5w=t2bB(o@!E`sEfNXB6HAMdM*FCa5ri#*iXH(0f|x7XP~>C46iPBHxo8Mlt~$00 zR0?`&5`bLxb`d8)#{?l8&=BpLYd$onh{KEp06JCaH~g{P8Z^X5FiMg%%lPQnkIZ=*=Bf!mDunP}3c7TTZJw_yT+ z_Y>gp$EckUpz9*c7JqIo+|P#u8cW;~f#)sPF;SCOqE@y5(SC{&XkM*n|%Q~@fV zO!F~cDaDA8L4NsC{jP~z=0s`E#-d=m=$(;H2y!_#^iL8Zi30*-lr(%rr5wvC0ip<6 z&rycaY7_oqoiz=GbQZH9>L0t8)}C(jlt~Hy*OjS4w{)6f`PdAVbq>T?n7MA*IoXmD zu_)Z-)mNl73mnB$9){dg>YVfDfB1G;SO9=rpIK0{$Baiq&}OHG_o7ZB6XypJxYfV7 z{I=1lNZGVu#AkYMs!8GS1@GGQl~V35mH5H1p7?|3=RI+!-~Z1^HlA#89jY0#PyoTa z)B-0DjfxN7tJje;cpF)^B-|{uio~%Wi8dR{o*>#839vwtXdeRzX&{)|E*ko%)8R$; zfOq8kWvsy%OV=+rn+9MJy9*{EoJLHON(fvms~F&%Ks;&I!*Uf*5O$I@G0I*r6a$bT z^#K_Rlr|4x&f@?pAt(9q{c*;vrd5iG_DjqT^*%)f>zxi%}|nNg$9ML%=SL z%~}&%Y2BH778gWOuM+c)c~WU`xk|G{&o)W+?~Nz~AVKowGRCNl0I}wAfY$_{37O(K zthIW2!jlG=Z34a;=SDLq#LL%#g{|F<8a*Cb`(`nnq2E3Qy2ZI;1d&jdxd1?1O#rO_ za&737Z?wr!nlAlw0lC~`CGDz81A`Tmwnq2qs)jU{f5niVUe0D_QcYL3(ca-^jkQ9O zvQDA$skg6bRAI1=F4Z9A%vvO(RO4MjDo_sXqa$tJ5f?JlOyBD4BT7DG!?1PH>Fjpi z2z$NtWx>cO=?o3b082l^1OcSc;ZihptwgkAs3g;TTT^WN(FGkIm?a3BkU~RW!tK03W=qvnW4k5|ZE(bpG(DB3kf;d4 z7DfNG4B9_{qYeQL697aXme(dg$Mj(aEp-Gc?#|_@ncV7zaNml~BC!ST=y3=cqDx46 zg(_-&1|FYmCK>RBonBjn6GtD*;0gP+y5HFAx4BeyogGb{5ib{YJcu$k}p;t-%E|ymTQkT_Rih z;ElEMnbV1`?KwC$T-QPRQWaa2o@lZ=ut{*Z+OtC)@%Hkxf#>aShzIv7%VGI0e`Tg} zFR47Cnb=-4Hhh(0SsG=jYM20^-zZ4?5jy6r6ippGFeJ!M^g>~9ZzjL<9TwGJ*PM69 z{1jrpiw$yWt|pfG=^#&%>I}Cn%P367+4g#>h&!4FjiY3 z@2N8;ASx}Ch(MA?5+%G*b!{SaOdtB?U8Ljc24{+x|7S|!`(+DrtmE}qOZ;6S`}6kh z4%R3KlaA0PdSK(+XomO2`eMu1C7PDAZk(jEFm_uo-CQKU%<*a6;v)oM*dOm3004n8 z@^ij2VE`^=e@-FH_VQ<(ujjFF6jQZ(6S&xZg%L-)T5EdWSuQ;DL~g*kf{~yJZ^~F^ zHHRA+=SA0bgFoR32LyhrlsP1WhTcYuw1Dc>J2i1!dDrGEWGH6Zo|G}DkrKaSItamD zsH)Y>Y1S0-*6E37Do{46TWt0f@eZl6#G7Ilw4W)l&4yu%atBnO&H(@e!EIHjd;*8O`1>(QTu7|`0W2u=@n>DX zbj~KV^q5wB8^vuO;k8;Pm$kk1Qm$RfZbZAP<2N*vd%!?xW+VUvUXhn_rU1|^Q=yxN zzz8H%6Z(=Wd+{<1e|Kv$S`m1_rG~sF@@-;#cvhy`EXLa3@f19`L?WYE-4vhAx5n&2 zybMD6Z^jtCXkf5B=ED-zT{;{*E^P?Lq_8FnvhCt~sYu zc}&UPFk9(3th(AGBzB zgz^je=;TMiUA!`J&>98yUDZlnJBrYGW%>8HoIiLN`|p3g^nmJQj$`rSs7Q%U`o}g z_fJv4Cy1(16A;UQmJ04GGo*h4v?~$ka#JKtNfrXry18JGPh)y5 z|G5j7(a~4-l5o;6qdaSG#$RvJVV0|CC?m5y z^yRZw$CR=;59z|T*~TXia~?o6kV`pHOe0PJaM6|r9(cjg;=~B=c=SsV>!iEZu->8%ft|-B z5?t&y(Z%R?OmpSk3a`wyE0|iUSZ9!kt!8@(<3Ji;SA(k6md9v{nYVlMO07IB0?!OxhnzkHV?tO(zwU#|w zk9!)y$fEG}>ivd8mCX=sl%dmyO7~b{6*f=%Y3ZIc#=M#`O#^4E!8fvKPt!%LCTaJ? zon)OY6dm@c`(jiy8S!F?$R7B|1QI{YP#siP7bwvWuqVV4dVXTm+R?VuMy%BCR;>@? zq%^(_6rQ>YeLox9+FtJvm|6urv43{7yENKp^Q>4?D9c-xPhUfDF&o#kGol{lz!sYws2#$72WWy5ueQjM+okySi5*;~d;1xN8D; z2kqL4t33CWwU)c8o9`4K27edrA{Anpl^$E`6vwaiq`xF&RD6GZ437{szn7R>D5xR#xpaA zflR4b0U2i()yDW9i^-jV;|^-zh2a!zhg(!IVLoLn4d&weW{Q&}kT*Z!Rl7bI^5pgy zcH3ZAd3B;yfUA^s&A&FBf;Vd0e+rNZ@o+E$m%#e(#vtmAMBuWUw7R!>TT(|AL{{c~ zIjgZe`kKYs&nqFRoN*0HCV54D3lFc<&8McYeSM=xtckD7w^jx!id3X0I`d54+Zp}7 ztJOLE=sEH`V*{b*k<1#$4MSBn9~7?aON%Ui4L)*SJ{9hXVQu-Ymi|1aS;8)7275_l ze5*P#f>B0YQ`l|0%CmdQ*Y;Qb=3y`N{f6XzJ2CbJq- zPb@=Xo5um!!b;)$iVvDOlHdm4i_B9n?}9S@cbE4W-jk)4VKy|^7qj?13BoPKQ)hl02cYu)~5C59!Trxm8g_cQ+EdP~jTx&l-}mLE>rHI|;2`IbIR&*Z>|e~42tyelCt zxtPP1Lyn0f zz2ZbhZ>tZD>j?grI_mPJON(B8iRbX>NMJj!e;mIzP}JsZbZla6do(JAAG5FSFp2i$ z-u1RjJ9F>YY%aw4>bHuusCoM8;ck*=KZGH((N)Ie-mK8GyE-x?iY$R_M<+pP%3dB; z*_F0W>L&dT-bXb6J{XEhCp<9Yvno72wu?%1-W@cyzL*dqlAgx_4qgauT)gJ-xD$}` zJqF;`fxjvNY`>@pN3ir5+%{lfO1MTHUqjEmmNpc=P!O&Xa1Arl;L<)ueLIz$K)E z%#TthU--U~(-BXp6&jyRKCCa{j*>{#vMitZuq$Is9)0R}O6Q=K;($@$mSt@7I+lq< zu^UlMwpK6#gy*eS`kS=w2Kel0o8F0T-cYW&K1ei=t5)T!FnSy=B;L0|9P+ zxu0-ibo@V;(Bc67W(JChQMLYeG(`J5k;9Cyz>{nBN}z$SfAX^(_@q?+eSvEEcd-3g z*4anb!GME}hdF<-MNAAa*Up!1=o0?xjh-oSCpkd-YSTl@L8qs<4g%i2d)pOS*{(Jb zCpq+BRKnqJx5S+F(9AI2QIJt?P<}JU%C*pzLHSgbI`&Mwi`$r8Lz*ivvbTEA}d%}Km$IFGh$E>t1MuW+o{wK2)B^d9b^MI|xTHWV*X zuqW=vG=G{;*br#p*d*(GwBxq@{FJ#%m>>eOMO+}5I_ZhbJ|DpBl1g)n4N)myDh%ve zEAKfmUbMNGO(<}>S~biXup*BpmH;D`eG9a(#Y=5;;i`9ZPxloQp+t}Bpt*=vW>||EanDpSk9U9a3po2( z+%Ea()pSSdyTx~F#KHtUrz9Ef*$+zDrbQ2e{=zE9=9bs}f!6$QH^z25cG|}_3ob5r zU-|ZXaMvX6Jsa&6)gtW) zHC%F)J4I4tgnWZ{M0@%N8@9a(X3F3??b#bXn{*g~x!Iac;<&ovVhp;C1SB%a@Qj=> zdu`3eJ=EI2DWu@PbN!O{UK>GMTz)Vx_ULXmPoHFbzLRTFj_=4^{YXx|@iOdHu+&bI!XG7`kG~ zWY-h7S?X419CZlj0E>|62L|}vlrMcUgsECvn|zNa9*mh#mJ8c*^ovPLOu2NHWuih%pq$b+5a>s}X2uUQJBjowLmU?GQ$o8En+z^fYt=`J;2k>uJZA_m{x zmja93b)b$cJK7B(piFFm_|ExS`7V+qhX=Wt_F6xaJn zw7i$Hz>K*vQS5EOsK(7EHz*<4m7u)hHNab%1nlY?ToFDjb-3A5jMq508$( zNW%A$K6ba1GoJltPgQhocJSbjRnsYG>ua?VDz1iWsRt2=vn)RxK?_xX`+d6g=2p0& z-FEpd2;9tgwJZ{Zedy)P|Bn{1$x~qCqi8O_No&PC(#m+#NjrQGn9eeQ0_=)SWvsj0 ze7vIX%~f>pMgv8YH7yFbRtYG^F7LaV)IDFNW)_Ut%w(5h!FyviYrCYyS{TuLPPgSkA*_wGcv6yVcC*HV?c5F;|cuIpFsEL(CLGO#HO+ZSyL+o^@x zR---=)GTJ5S$;KHkvf%zX!jC?N4_gsd-(l8d5~A`hWniW%s2l2$H=}whDTEsTk#?K z2&+ybSgO1n337L;cezS=m>M{x-(+>En1r~N=$ja5b8*I$`#VjTv|b7$+M9a+H$lH3 z2J`XpU75T7z}}1G?@c4i+0I#F*%j;?l&haj=vV4&HLfwL%2(kWU`ymXj2=G29fVaD zata@A z+if>6v!~XL6JSuWI*Rl0DR0fa7-#u(Vz1OlRiw)6EM8{!$;~VApw2v4n;jCg0G|}O z7>kS;2tQ}yFl5$Bge^|6i|lU=xZnU6$IWMBJ@^~QQH9%of#{VLp2H5&MJ=f(mqo$Zd#$iUF)(^LtZ8WC=O1}?KUy`~ddh@G4j(Vffn&ghM0cX`a3xl5q6 zP-hwu*Ei*mD&frw+2T?^QfRuiSiChgt*VV8H{0u9`U`(<(_i?+h|7wt8s zj2?a+7S8%L7`x6+#kt@BGr!9CJ*5XOo~_yZjb4DjZ=0~1A;;PEV`ptie%q(XBu!6y z&kXDH)g5(+FkRb^;(p<#)+_G%-Irca^5Es4eVh_GrPZPqID5%kbphzqfewopfW8Sa-N zcrDbU%~jKSPqB<$P1zitRzGFh3qL#UC__F!Iy@U~RD3q$vlW!yd|CPI*C^Fb*LeD> z;FHX{ocbYp2vj#7xooUf5rV?y>d+9hb%r#L;emc*3w2)l#rib?=JFYE$lAV;*Qhel z$Tsu!a?zp6ug_ine!k%Slr5_Enk!zHr$B*q%3!!v!ahQ9GrRKt3=Kaw z>6c*`F~L&0uWMz&u8JvKl)%nFW~_T#pkz%_qX3# z_O!QGbC%NHdMYJ;*P6Gr002$$)={@AG+%(gKB|M*C@929GXM!< zPUF9yA1C^Sn~c?U09LBoi$N6_7d22V@m3h@K^BZ^2aDO9q|a0blY1uDLsLG?HB1?? zv#=ENeKj`56;GYZv-S2OM6_!O52B(g$dQx=GE9wX(J+5ZHhAuPIgO*=X5F+WhFo4? zuPu2U?_3am-JgkE=Cu(=(SSn7FoIx03(fPC7=Rz_AT30D2~5?@;P}GkB+4#Te80Mu zAPC#T8)N+s+DS)90~zK<%})IVB1CV9PN$NpJzDL;o^?qC_JQ}qP(42=WGcB1y z8V%h8^R3rfdr4pDcdhl~gwoNPmv&iY{xpyuV|XkmbdUy=f`<6miUKz-p=&QC2FZO{ zbn*lE%Cy9O&jlbstDQc7p5jJ9oVh6t3Yeg8?8~Jw*v67#o|CV7Qdt8bQ+1ygc07$D zk;^Zrm{Fny2;qRhTTf9wgyRcjOU?jz`sms<;z)o7(kb+U8udgc9y(mA`Msq*Av)$B z?xoGzMABorAJl1&bvS*AuckSBAu$wcbE9M!Ax9xqehg&Ux@ z`jJZ#3OFUrzX#APzecrzKl5((jjR^=9JgY_19h8l$WVu}QjUh8T@(-#j6}mA!09|g zf(*+KnkzbRd6W&q$|slqAjv4uH*?O32?GU?n})!CDe3m4xRB+$pM}NpyZruZq6R$u z?o=xGhY*6cZtcnll~y)k{44SS98qJ?ijIF=e}O=iyFQ~fUxsp^EjV9M!i_^i&~6w8 zB0(p=eg1&PS{O%kkWOOolnmO(AMDZeLbmcZ8~8+>3!-0IMCl=&nPORAP~F*PlR66b5}G!dC&U1B z(xRK%Pwc3D2*UQa!QVkqn|?4$LLG;~M^TnIg$V#Qo6Xv0e8xki8sdaBkV`;n6be3r z9 z^^-;7sI9-j1%WZD7rydi06Njp;g9HnPT3SLcmNvE5K5EE+8c~9K}hP4(*U5SQsNQo z&-MX|{!RR?v5BLGy}GqArsWQlmRI6}z|FlqzfhlVFoLjU!Fv~^iYOhsM=>y>+_Onh z_&XMAS)G7zu3Rxej~Pf7swm584T=gx&~k~J z+p>g(U#$043|i3!NIpR+=U>Tv^Z}A@a6sVZIg=NBcB~cR=3Nsv$M!`lHUJt>DMk=X zZ+)|aK=S7jCIGl-M*t5r%nZV=|H1MF4dXklBs3J?O`HfF^H%3)5(05LbK<-^Y1fm^ z8i<09@|ctj!!%4BjMI_JoC+0zXc#Y;B%z@%47k3aVSHp+?nD06DF-`Dkn2)ct?u|@ zt+e5O<;4I<(@8?JZU0_<5cZ)vb|GiOx&qWOJRA<70a2jXjq01c1d=};uuw}P|9>sX zY@_*C)x}Q&>)Gr&Vjn(_FR7hp{6LSx#e^6siA1M7m}1~>&h?fkSi`B$cNZ=F`fG=0 z!uFBz8#$e)5R=U$bI=39q$>h+`9RMJ3 zb51uuNRnm{aLEJNvR8V?JQTxct*J0HSpIhWITs9eu%8vfpb*;)%ZiGB%&(-d@K`1H z2{4H(Gs#j-H+F@wulKkhTq+pWfqkJ=UBuqo6bMtyE3~7tBWhYd#VF4z<`UuIu^!Bd z<=YN4t|cAJpk4Aii+S=)SOd(dK7CCIm-=!~>ga?CLWZCr+9|?}9-(8ddFpTX4QP-{V7={cOroJP{aburaL+hON)J_nrsJ^>32& zr5TnGyI>OQx-D;_JXJi3$OJ%HKEv`Z@|)FzSqlGy2M(?xn_b%EA9PIKyK^16{A7a- zQf6|LbhR z_JgNcYaR7BeWzFK42OAGj11BzABEIjJGXwQ>F9{zwJNw69;tdeH>iGAu2~l9Ggj2J z)*CAL1FmpP{d zlGe>}3a*s4A9r-q@~kHn@SIjZq+rd`(q0sduaXozP*BUa!+$lCg6~`0jcv}owIVZx z$;>vpQyAlJ`OTyARZ`$BgHqP0P^b`ql=S$U-}sguB7_aNtFHLSqLH7y)|)wSD{P!P zK4MP~59}TuT1slDF6{0PE@e=ebtMRa3G*^_!yL}ye6H{m(jvtelwP` z;43nXk(EWeZu5jLIA;OVeSr15Swus?>QqNCsD4)Abss{_h2?yw@g6C)ruX zM0tVh&ZMDN+jZnL=fwo`VQ`95^0cGrUTTD=2Nywy3<8inU}!u^;Ri;AOgfAFiE%XTyQ5~!DrL`S0+y-! z&Z}Kdc)hy5TRklS zDiJOR6|YO`9jBH#rQhSM>j~6WeFsEVkP27xsf)``PFvFc0N{&uYpYq!?PlS8sR(CH z8lmtlqCG@4nZ;!4K$Ur)eN^eG;~D}H3X81Jd&oImgC~TM;jn5=-*krel+~yOL8TP?2V_0PGx-IZkN0qVnTJHcw&H?9qw^U=zXv1P$ z-OO3qz&Co}uKV71U%%;3Mh^AMR&6)KrgR6~T|=2&?S_RP~ZI$s^Q(oscb`g*#TVoR=Uv;8UWJfg$k<2r; zdz=gXCif2sL)iH7x=WNC!QqL~nig!yXHQ+M)C8L7*LzRCuBMw>s>VLF%uI1O+s|!d z&8^!nwKAUB#>c;9q)wt;Qgz&`%Af0N9jy*r^?L9H*@7Ya1nDqI2=OrYQ$K@m(!YmH z>2w|DK^eb=&KGpwLL@=pw_mHbzxn3|;jH_~ht4-q-{tr_=!~V&11*l%;~#1()SyVq zN1x@*5wuRZL9vpA-5K5&xo4lhK%@uBOglOr3sZB?_xw3 z+_?3qh&bF{Sj=qXR-ecIRM6S+0j!Y1lb~I!Vd&LzT&JJB?<&m>*>XD-1Gcn+8QJR( zab+|b-kUv}tj9j|1g!ZBYj|Mhf`-vXwp+OS0iDwx*IirW0C*sU@e8@%uA{k{)~ty) z1U=Dg!TQS3qaC*yC^9*v%4G%K7FjC8XUA&TIL=dOWhm^n9p*~$LLpU$gHV< zHM$OqiDJxy-9xtZ$+ftXM;cACnh=j~%0_i2Ox!>Tw^x0&C7Qqc`ImgQel!bPo8;32 z3CcYkTieQEYVu7x76sail(Aeg&JA8CiQ8F!GbDU}B=eLYqq%Kmukxkd;V8!HiKkwr zmCd@;>lzXMiwn?A>V4LA9DeRCCLuGLzcIJVXz0;)w zm@*LdJgO>hlzq+88Upitl$C_Qdq@nX@CaH_NaATw-kKXZRoc%w0KcFD&zDDKB}KVI zza6M8J#X3nX?7Cz>v%n}W*v#s;JV+q6)5!?GQ;Qu4?3PJH0E(_*L3l3oXpYxrMZF7mr4-l*bTf&@vrRt zgg>W}QluooB$icSNosNcaW{y}Y%QH)Pt)0`OdFN!gTTS zXFXdwMOAy^ezTa7QC8fEC>r30^5I`zV#dZh30!o%I(D)DMbc0S=!s(tguE^ZEnb50 zlBU28{41B@`MsYi%o6tu2zxzL>o`0;vnxCQ$So>Jf^Ri@aQ(C50+dc$PK-PFR{S>H z^!~Zs-aOgUj0T#P-^x))aEONkZ3C}WG_A`6AF%tN424_HhDB%=nbXd=t$g_)>}sUR zq{NnhyOAJQZNYOfXM5_}RTI};ktM9cc9rWwL=@SU*``hE@bqrq!W@6Na)jvj`Q$=V z)#?`O7;OC2+|}~-$?@!^aHP9mtrKUlK3^M76n{8LvDO}?ZApI#XH4`oF0 z^~Ck$`(^aQeK*xVOeG@U==R2+7(g9&wOGHPVK}{#B-7;9Z&I*H;I^o9jwlX9zR}Xz zywl-|pW0K+Uy>H3<}8g*jO6d_J2u8%Th$Mz#|K{1@NciHT2gf$tXi!_;Is*WH(WJV zrWf~RrWCiNhIb|IKDdi~k5m&CNbGVpk2~};q;;8c;Pu}hKxU)ASvx6LUbv3PV7$pF zWZn|&F%_R0#|%x9Rl|E-DVRDL!xnrJoK}}VMKReD>`6n(_b#T){rcRX4Kd|VcZM+b zaK19@>98hTI)88dYJ8?o*J2uOv0?V2u`ArIR57qCJoR714r(8YqAn#hU0zzbDGe_c8YAPG~IZytzVh??B!@N<*do%2=XZR_|-xQ)AIKF z&*C^+cdF2aysgvHO*z$=RaJUzugt~D#rlwZchd-7;LJe~_T~-6$qk#_!>}#0QmkI9 ze{sgMGf)57SNipbp~W`RQ)&qu#Xgfq`dV4k4@U3ci5XIliXN*`SzoptN%UQ@PxV`i zh@6;|7OCisi+*WLo;ZM2VNT7c^4uaQ(A#jBC)SaYgU_ zUnAXtXZV?ZYuID{_-CL}f5BWirJe-V7LLVc!`93P-LI*&r8+vZ+TQcZti#1vUQk}h zR(*rWFZm01)7BG7ArdOL5tyWJ_JiB1YS$1w9&lk-JMs5=nB4XAA}!TLdyOFobT_ae zgjCEmHNh|MaDdbP%*=#(haYryXJ+};yVO7^>&bhAuGzB}MBG}xzi6u7)OYZ(-0gKe$27=m@jL&f_x8E(@r*w( zdGQI*>`NHJMu-7d$Yup^R?|91Y-PN3}>`ZfkboV z`z!nX1_xiu!99IOkH|M_r6Bsv2Ct#O3Qxz2_d?TWI;AiE0?(J*XUAWB9m47z&``q! z0esNmfdc}{S!}_c*&|n%UR&{W#>-{l8iG_1u>G#GE&?$VB?p^3N9LUm$sZX-br-oL zc2`l|yqf4weY$!Zu(glO){2{&-t{3KBKdG$H&rZs9pOGLwdrLGf+CuVH|AV+p|>OHqs?zw zS<~rDQzD5+5Vv%KoV-Wtk+F0v#iy6@{oYkB&F{j!eJ|@|-$0&u{+sSWk2^Ru zB{`P=3TLdyJ1XViYao?(DR5`1VCc#H;AEtp4s_UBV>Sy8CeP_O$`_2hxK5dXlSuTau=S@68rP=n6|Zb5-fFpOKq@PEUwC=Fd?H{Z>R zOudE8X+k)F>*MzOyZ6<#D#X^xrP!P5t=Golw zbrcyto|M~cnA=va%7TVv@3B86P^|*qfkIa=qR*d0Zn?+_)1#7~0zIqEiH&u0PR89v z>Q#+LSs|0Bz=bpUDWhq8;5Fwr@#k#<_K|x}cqiH$E2+Kfla!0KQr!TJq542}h>$&| zjhiScrO`^H1|KKFxYnqJoa}?Lch!=9QtmiAQEHBCAg&@CzKw3ur>Kqdt=Y`XH8u#; z%UE}l0BnSFIiPi$)Q^2Cw9dx2etpc+AXdC{_dMNfsrZeGS``KOw0x|wT2ouJM$f{P z>o(*O(B%8UA>t)j#O{h!Mj`RcI2*fT5I(SxKDvV4(Zu`cPRWkZc6Qv7EiMvG4^{~e z^vjIA{vhlMFXJ6vD0G!$p~Cs@@;-DjBlq;1ERIoz)mhs%yI}8+=kzbVDk^S9&Ll)| z$1d|U3b_ryBAfRUPA+t}09yvzpFyVPb1}oV8b%X6SDPEw|G7H59HN9ShU_MM{wbb!473W2fsf;iXczz`J&%YlKLx)S1V!JqEpKkBOto<7y zVUg*LUdG5AFs?gk7YSa!(>WV>J7AgR>(Cl}CIcN_2n%tuC9tvo;o^H{OmJ9JX0v8%OVjz-Wi~`5j8}n|{ugWT`Htoc9Jc91rf|+=@Z;fPL3*=1#|66b@tn7iQ0j zTB!yMBEKPCKG+*&J+&;=@EwAw!bXkDT<4aOWMPZ&30=fX)9VTUJ&u!Xd(NFnH&O?+ zR4ZFt&8=GYF)G8qMAP)>DJBz}b)dt=o+2{bdm2opPece{=p$@jhmx6SUQ ztA|Yx{gGx&wR)L}i~=~gvAmFLbb`v`G}v*HfCceVN-9oG7UunPlGEF5*F@};;`m$B zm}{wVPcz@)oJ`+9A9~tr4(mnQoL!Q1plLAnCf3aCkXxE|h9YTQM&9*Q%H!-$QRV{N z?rE17*XlCmBiz2v-t=+%JvZyy@h5-knjtq$hTNX__J!7(Bo~pnE|$CwOgzgZ+3hH^ zb8}10IH)5Q7;iYKTumnTbgT#@`TrPs%c!`%rT-HMK?99LkjC9f@W$QU-7UB^4uRnA z?(PJ4cL*LNxVyW~BENg@bDx?2teLg?(b+&NT2#etC+y84)5r;uBD)~V}{?SlJRyLR_~}*3D! z>5GS{ftPSt$DC5*%bYxBI&XyWM6zt=j815eG8dp>^e*vIdH188=l8WoM~(5e+! z_%HElysguK_Rn@2c{EdEq`j!~>Xnjf3py|q=dbst9A}lkjE~O=fbGcpCf+fGGk6r> zGa1M{j#sI&X}=ZGDPCYL&||ZwlY(sj7t;gb@Hb$CQ=0S<0sQ-g7_FQHI_+PaXJk2@ zFMb-!BxxQdc#nbNZ;F*PD%V=*={s?b_EX;NS}DCQ+6KF{6<{(EEi1VTpJ?w>nGiN# znFcHDrLXnVxsh(EY4lrA38UUjQvAVw5QFI)ET@&Nw+?>&;APX}gC9^gqXmkO2PW7&MhM6H^mZ0$~GDTI$^tOESl_)LveMJMYZJ#D>#q7BKlifmgoZ>vlHJ3B7o90~X)Qpiss=&HG`nQk%qe=&`10fBgJTL)d7j?m zfQyxK&*kLJXKMfNWR6S91F}Aq8^S_Cx_Wv`d(V}~a<9(EW#{@4yco2K{J6hMP9;=U zxCT|glXpXPm5QS75h(_%FocUaXS4U0%@n)j-wj+D!}jre{I$k~4c=n_&G*iE1i(^k z%X+;{35g<=2iALo%A%4kVW}%V54q!S8|IuIuJ=Z^>l~4tX~i;OfiFnAyC>YH0;gkh zX=5bnUc-wkPj@1Xf%TEWJ^ruKot!=mg7CvB%aaiNb|rI7 zsC5_c$DRhI$A8uA&!I_y=@e8fu`|NeWXsmRM9NQ3{}kCgz)vo6DZ;uN>rt=HEQ^kW z#>n3)qvB78fQJ5OhtsS9;$zgb4S}awvy4p46$uHkS|{aE74T%%5nRBl_x5n?R5(|9 zv!{saDVKR+Gv`|8+?$i-?9Y>@e6I+H+!+%m%W&C;nR*nikB7fkf*9RvK~4rQ*(qUT zi9L@4chVM$BR5*E($SL&<@+$O?eC88e_<~Yb+1MSd&X)T0Cr+RzV%JY|NHHrTaWt6-{$UH6 z4=JAUA)(vI{?1WCYybH#$25Ab-zd9EwMPR2Z+#(Yf!XM$*XZ%r(#8;l@T-2`wGW+` za761y@5DwoJix{c4Y<6S%LE070_=gqAP1dm`9VPOV!hWxXOhPmN97z;!wU)uKD*2#^nB>VfFoMB zdIyPS%qB)Fv>M4B3qQqzG~~lqOUo>zT2#t1wgyhPFK?bZjUQy^TUicn-~cYl1SW6* z8+!#DNN7VL-*XhaL(liN;EMOcI-m7iDhriGymc&{1sZiMPgz}=t}1XiTJ=5211p#1 zx)}N1VE60e|8WL>Pb7c~p72^@t`=q0iidi~{SF|h2o zXy+#2e<5;e|1RV!z^ct8EVx8g{Cyb>?6rDy_;t8HwYuC%f^><_ZoT+3TbHWjqp zT`(m%d;I@kFlG=}+8xCP!tVvKo>_E$rSlTM*AgBl>Y{;+?Gzz#WW=DRZ`24xzP>ts zi#`?cSxJ?#1FSR>aj6L>nY*f-|9-wr4C|xk)h2^Ljq|59#d2{-3-u!R@M-rip!bx! zllRZ>pr;G-Bd)*NCn%lIRrFAm zp5_LZE#W6-X<%!osLUV9T;Y&D8_xEw)~Zgj&R=QN72Ma+x#W_grvzFin^9|$D=;!U zFLHtCK<(T2ciUG7vcb}%k-XLG9f4aXf42jvq^hAI!lIPayOSwiQEVBHFCeE96$MLd zMl$P8+&#q=?2R0cCo?jMShCBFJ<p9cK7HQ&V6E@9W9xmcXW|l)&?Qe1|o!6;b?nWIif}-9pHPR0zDcg+d zeUBbh6>jPs)Z{9&1?mP#)*|m9hzoStmZt!Tn_bcuAA9S{S)6okED)4!xnX}|2hexM~m+Y@?{RFV>yv;pc zZjjLZ%pvaXYKut&`nJ}hz*P^H|`FEmXrP6tN z?4!fct#ye?`}cTS6*|#v?H{met@Pv)>Ef3r5b=3-V)?|2$}zDlp9Vx=Lg+fgs^Pw(+iyA6Zk+~!=svy7%o{Eb!O zXbmasIBw9$oK;Fp=rN~5+Eas;Y?1Pd$H8~$v_??6S{RH9nf1~&i|I6|Dj>+uh+?SN zEiCt25)kQVWZmY(x}M+%3O~{LUr4a)sD2j54+1+l;yl~fa?lwJA=zVU1&E)M%GA)ys{WN}o@ z+NU89He+b5(!y(y^U)iTXQ2H~zvWkg)x634hEYS6Mh$o)n&rnOW$=DLu_!Rm`?X^% zgH^@UjW25`jd70iU z0@UQ8tGx46Z;duxVb9ap)0?UDCts*`>GGA5bWB##UHI#z-N};57sY5!DK?4N?TxT2 zRmL|ZcV{c=jFy|k)v69sm(iR3Np&YbmwZyd0n1Qwh@*_x!1J@!P_#rXaPtxjYA zvv+GEi(#NqUGHzsUq9U0Y^fOQ&A0Dqj|}=(5G|sMRVw2b6{k7Fn>+|cNPWA6-d%#< z8Z0zQF!?*(`Kz(iiZ~LVo4nCxEf$a6Rw4s8K~O?{ZS~L2R7~gs0svRrVF%o&pkg)K z_qfX`GlCImn|HD-SFkA}ID*l@cWL+b>g86c!XuHvmf`t$qdqA1?b$5h`eaJ91IVf| z5??_!tdt*qy>$aD<;zr)Or&P)9RMv5)8>+XYi56RW(45e=O0lkB_{M? z9eDoMTj@ny)^K{w0SZrkOH1#-xET_f0yS^fo)Va>MDn;4lhWm#V7Y8}pWZMWQO)G! zIqW>Nucd-JHRy7t-nVDT5Oh{T`16IG67V7!jQe(Kn0TAj~or$it0rmgkSI#LgyH%try zgD$gM>F2<1+4$TZygkVAq>iE*k8}DiT9>a?*lNOEwtqvs&Vz7eXh>)%EuzZ6priT9 zVOvB~PTak`JKeZk@{yr0Z|&R#FK{x!LRw`fe>nwDVp3f3Em^fn9Rp0NGJnFmbI16POL zjL0q#L59MH!s2Ns9;3lW`|_aK@n7gi&FG6~$wW37i|;gKif!F>H4=XFDqP^xA}8AQ z>F}Od2l6p2Z}r!VQ2~zr%>szC>v)fZv9YDz|MHKJ{nRP~MW*ixjV}#aAnP7X}(|IJQ=umv`;9Q*lFZ*Jt@u{Jd|5y=fW-7*Q^ z#z50ac3HhCd;{4@wsbL92VJWq#@Exk5r5A+;HuJ$ui8xnr z5b)8y1Zdgd>Rt)Q9)>iB^Bk~U&yOBrB@ncjZ;oa&xA^;wfS{>>W4$r;>WL8j{w5DU zT?Q%jM}RDe2)4j*@`2~~`_T`ArUD+@O{Z6n7UFw?uhs()e+cc4f3O8MA^x4?Co~n1 zZZxSrEeN9D-;LuZ)*kuaIk5hZb9BgM_0FGtUfvq~;nBtcgStNMFOZB+G83Tiq2}Ah zr%$-dpA*XFYZsDR8cL#iVO@+iN(qM2>67}GYJ~eS0M1KPm0nzp)zEssC7{Uop=5=8 zH@htgmkwYogTcjW^&B?ZR|A{%tuf4wF7$b*{_grOrTzPJK4gbqzlC%?e5W=0w0(EB zW-ygLywK{14txah-@~Cs^9o^owONE0k&2&Fy}3EUE?57G_lLcz|E0ANg~1b?FS`Xf z!4;uU;O=#k_#?lD^tErKzZR9g1&{576R8o0d2WsGx;=`9+%ZQuMUqORf#vo1;^pfytacjUYeu!LO&7x2|;1yjVEG4JKz;R_#KGAk!8fjCDs7T_s=Ba!Y z%I&eMw>#)h1}-<3AIH*|f!Mz?d&-g~=F~5@8AFj*9*He3mARhiw#sxF_!J8zgG_Xv zP#Cm15~>U$6!)iGsT{T$d|%s_NUUx_#Vgq#GkLteQyabYgyg}VpPt}t|I&l@f_aSy z2#GU4db{gGeV(hTHJoohlb^MnR()~V`|Uhist&j4xHGtHk`|G2$}}N$!GD3W8x~DK z1c_$%2?_%c$sC0*A`jI-2jum74BmC7U4NXD3dZkO2?;#o>)4upn7MzrqJ;UB6VQC_QkHl4n-Z8`*`gA<*&B2xx!)%jjg!QX zt|~|pLvLU*Ne0;1tiIM3fWo!Zb) z#*b9w)4!OFhlt1ItzQq5iYM{bq5-G(KF5jDK$32`y&RfATo5@oT50OoYc(GbP3Mp5 zvuj0d`fYaGV9|0*LXqkJfu;g}^YC8ao`GNZ30D8=8v6K{L1Ka{t=9y+KqpiDkkIRv zCaMxcq+nRS>HvMD%Fwr;;}yNY^SO{wJBUm|sn`>H;w9{M+-h?x=q{PdmGu()RmbBh zn#Gw5blnW7(xFsof3*dQ%IRi*bU(eeulpviL6D1Gx%wHx$qx2KLCD?UB=m>*??PHG zrzdnDCyU5-ZuUI+DMcJsdxqR5QI(r=T{gKJt$?Y9m%RjtDN5kBsR@6T3zb?kOkhMx z$ssB+Yq<}*bFuBr1E4P~i)rY4u_^_lC*g8`2Rb{-3VqTYB%_M3y|A zn&dIJJ^Dl@uWBQPLs*UX;Dz1|TV)}109@`j&Z}Ih%eqSArI}Jt6euszM>-Kx-g-!Q`nA09WXSvt_vFNtFGfN~Fc?E4RE_{c`Bio4khkv#T_a@yLVgfjA*)tYaT^Kp zb0(-KX(B5gJ^>De88r-xo=a3FH7b(clDdXGS8Dn`=0mmTlypT%YWQB`8j0a)xII^Y z@A#40ERsaW_kJepSIyQ%a|{3mz@=7wQ2~tz@(%_7h5nzb-qjZp{gLiLoC_3uGpPqq zJY(udxffvBhbnwee<{pR6BXDEl{5gc^U^is&y%%UBpz34rz`1 zsDW@ehQ!g8{_(ibt6)Fs%1P2$z*=08ci$ zmj_28w!qiyM0BCfh4XZ!{f8_MEY%F70pqUZmD)Fv>wV@~q{w8A`qKrx3hvu!Rl;;I ziqg+#4K&&svfN?aSZa@jSDV80 zSTn}O;&wNbi)f-8ZqVqC(1u|_!eYdL!IJs~3YgP)I1{gTa9q(;nfLCD>Vk+u!jJ`n zBjWRK?6SmPB4x7T(_PV<2am;vs+ljqnVf!%l~|^-@na@BwVbr zLEBapwK4lzV*2Ad&1LMh8V)9ns^mkEcq)fGO5(4q%JHL2GEPsg(;S_o4)dwX*4s0G zbUP6s{faRIvf@7JFy{ zNeoj=>u=7+w`%xe)mj7Jl=)=(khp3N#HUWTXmfs}pu64?az`f1P0;nhk{~BB9xd>z zw#`aYsJluS_1@MEw_NcbmBrL!y zE$ZNJlT33PRJ)| zs%k;T%@*>88=dkMsc+s7o1aVLM*!$Jyx0L-qSu3oNVHS!6O$0Otmd9*P-&PFPq?)OYf@@` z$<&Am(TnZY{%A6JMRdNx!H3H&KviqrV(CP~fl&YPqol3zqj?&V1mSW^#n$r8T9+s0 zovunb(BwDjd!KJqK>TMyL*%G;8~+vo(eGa|9(-~=sM_!ElLdiPk$RI#thO4R(Xs=v z)L`K(-bWfDz8ync>u{VvC3*SA%8umFbTZw-&<7Gj?mYNn(x1ebWxrM0h#;ofXm53W z%&yH!pbi>Z8&7{`-<2gS^9GEM`^RH{VPIBzyuSF5SiTZ*o$`~q6X}8zh4M@$Zm|++cWC)#CxT3dsq(|9<6yR=IjF|mWp&hV&xh( z+1JA|HAZ=w&LDRm!DntV`b=&W5lJ+^TAwo)x(s$C$0XY+hTX&)+KuHD^bumM0TQTu z^$-UfcCU~TQVJQ{)wZ7I8Or$D77kIfkyNE0jtMnG^wJq4BFDcBmg0l3B3`h4H@nph z$K>9_K{6B?C=5VO&)(z@AvA#68E@CnkFvU`0t^21>(`yWH1;?#6gIEaSMTCEm3lV=Y4eN;DawU(oKxj8*f$Gll^7h# zWy^ua6DfqON31Hl;t@0!6JULIt}3@ygLHtrwbaKg8xR!87vq25XX z6XY?chV29JfJznH+%6Q5E~U%pE0=y&bmFYhYO&U;%G&JyZ5I4O3W-Jp2}{aHtYYDc4I=SY-9gD-U{=s9$@c2ka|ewTWcaDQKJ$g+3ILSxfvIfin9BFz6x3GAyA zjb#n+*AZ~e{w0gUiZY7zOMXLxR;|jcCT6&7&M=dlLW_jxm^$ThGPhCRr{v-?gZg!J>lT{d6 z@Q?CAr$B%$QYXRB2SN>;5-a&G{QxtW=+g=PH8TA_(Dyxc))R8-*T3TZe1iJc`j&hb2M!vXa2Kw}P+|WEHR$)J^ZxU_ zLa83;9&|Hz$p0nYh3gT#+5K4!Yf$>{HTj{~#Jg}|3GwjRT7{uZN%bRmgfH|P4h%)! z&HnE%hj{(zn$z)U&H`X0WSOAw-ai0B6~5e7(e54r9x9?SUfS}>OgNm{AZY^Ze*cnf^~is( zv#T8X6VLrUHZtGakssp60FAxx35%4FLU;1!qGA^)xbAdP|K0$7HmE>+&(*d!UsIuh zjYA&7U^|p4B8jWL>)|;4?rfl4*r8>EcMSan5Aqk4@q_XpPMOT#63*n!xiJ z5tbas+wzpaoAqq-Qc6;uKiPy!J%j+QdO#=)1K`rcv|#eDySEdDA`S`a>JeDGrzW~a z{rP-0@toOJtIgXNkpyAYfG--b=3VQV$EB&xVv9==@i{z_2PWI1?3ckbte9o|W_OHf zoDuQ+zQ1ohMfTaQ!2Bf3oJHR2lE+-RzL;*6rTuc#ZbZM8E2+`$@scMg%uUy$O4wA{ zoA|870-E2|N3~x#eIBkh1EC_dc2B=q+kEb@>^BF*N*q+CI;`}BBR4}|JQ{M$1}VB# zIjvtcw`tX-|1_q%AsF7qMl3z2%~y3rduP-e-M_wYCyu3so}|t9%Q1&7TGQY^agHOm+%R&4LM#WFh%=BshIIEHu$!=PqC zqm&3NWyQ_*o@s!?W=Q|_3c#Br!237VOSE#bLnk`+^ zeqp!)-FV>V6OxZ8_f*$>QE1opZu7XpO}owX(+W=>kS4x@A>qI1A{Y3NeV{|*%Ycs8 zS_yc#zlu6F;!r8Hppl4$d>48fBZfs6y~xLL!kpx z0gnC@XIjOdlUty?+O=u$JOruUVNMZSARh7jBXsMkNN%ClD37b|j*ty<&XTRKu}W{M z#)SUaV-rpo?!Uw_$^~(*e%Emp{XSlnux8=-L}tFE!<}4^}Rl6CY2+ zPY$|1`i@aDoo#hSta%~dq;4UdUJqLMoS+pw809%@z^8iLba+nB3-s$3s%LTAaC}2< zbIIhN-Y!x0qKqQwOWnCrC|0K?2dXlr1x*P(5!HCGNb5BrOT-q+KQLrIVV|VUE9_8nQ*qeP7M(s-4GZnoA5_1Ae3dOg80ZFZJZj!!It`26F2CM_j7EgQ270pp z>r9GI+_i$=Kw zQy5fdql{^|N=hL){>Me>6~-@)Vq!f;Nq)H2#y6`d`~@*=S5_ba_lDW-p6p~?NdKbR zXw?1sY!RmU3bSWS1n+wt@uslwNO6c`LP}z?C~KvL<_f-eCKJ-aF6 zN&q3BH+v4)ha;I9l=;l+4CE4@gB_ryt}+Nl=1-1RR%cyj z!dMn^7O!`o-;Ap=cVzN=`qAP4d9&28ghrmg??-h7o|+_M603 zx6AkK(&X_K>N*7aLYHphnJlcot@$cPM#g(9@YL+C+W?lTRK>_kM=toik>f{D_aLwy zD9pwhZ@vGjs5E_<_dTjgIotlFrv9Sy!%!n(c%pPXD89s%wJIkC7a)H>JNbg$oiK6ZN4kaR~Ka#FO{0Qokixz*{ z(Iy4T>Uu+stG#8u1d>|T?L+qYe>$BhisEq4kNWw*=XF`^fR!{`@nd7cARVoaIW!{QSgO*upvPrr0iMAgBzZ6wkA8wudt&D6-^+l6XDBVWa;Qx;> z{TrNM5&tVefi;x~!d`^vqqN_j=+^6sAJW;WksHS9W@_Iw4Am6-r+w8!>slA{D5v5&*(b{ z@M3a|NxEetwQKe3?``|MRb%(d*u;kHC7MijYARPm>`1mf!B$v&)8M`fc&^#5{_x|T zT}v{Sj-2h7tAzefzUt?plhLJ(348ZPU0|sDsV@UHq0g<3YS4^M(GDDq> z@<;Y6oq^4I4m+SHnai6>Te~O6el&8S)(bt7@Ig$(*TFF?#C&Q+ z5CcGhFU)U_U)cYx)TZpZgDJJxp9PE@xv z{X$ghHB|qJ(I4RMdA*SJI?e2&2(eEuHgS||SjQ5boQ)@ghl{n_b%VX)g&G2IR+?;@ zA88nf<#%G5+CGNn$`tX%`%d18YxkO-s05JfYzhc}_mr6rhIQMQT~$+OvLbn1tVQyU zBBaS#&m{XS8D9~;Yu!>BbU-E%pJ#$-X82Ud3bs3obSGobd6Yh!VnwA*#n3MP?N}6* zXW~CHa%{d5xn-&S8b_gA_7*I58T}{x$59N4islXneN@|vZc8o=g{_`4b&3?FoJ_IS z=c?e?w0TBS-S5KYGc~Th<_avQ7U+RT*RJPoc53bD8~Zt$GN|h?)?a;6{%G&?ZGx^AD9+Ev$*359hZxw3r<#&=GkQp|D~Z z8j)fD6?peZf|3iakaa`uHNc6ZMZi+6+8rj6=Uc7haPV-OMe2|N#d?)Q|u8N&klf0m4h&a|^G zbfLc?VQ}Lj@?L>EhOk~T(7XbCukknZ{%71D%Jp{3#1q$8o;sR$H;w^N%^+bj-TFrY zhWuAz;(sJyvcoy&ftL*PP}*>Bj=CKanLYyKyT?3P_yOLZY5q~O_3zp7$eAoJL33Y1 zVevAunT``#k$DX`Kx{HlSd{uIrtTIcsnn{cz@^XABj$8xW0h`>#RQ*xuBLPQ z%l*X`Y|DIkAhx%he32KWa`~FP*6vE${6eS2hhKzzfns;38!vy@;jkI-b46YKJw}v= zG@9T-RZG8%$B@FBo9*iP+d0i6&)WO}Cg8G7{ed<8tDhX-g)h6pU`f zQZ?y}@Dti3VwJ#f9FUpcYI6(t5_-VttUS{(tI7MzvSGiXiH*BnPXM1g6)_sGCzwnk z0oSI+XlwB94D9p<)V+Tw@eGM27t0!+L7}w)S!-bc+-tskfRFb50WS|y$EhYzvke6| zx!?KW+CO4QoPf8Fx!{w7eBblmi3E}$&zH-Od_J#zwy2~%{rw^}lgpq!iD;CGERd?+ z^UkP`-N9DS-M(@GRIX@Lr494-3h0|o19X|Gv`C7kvz^_jspr!zW?WkXk^5M(Kx(xA z3*n7bStntQ@yFPFY0OPe8Ny}4!MkH*B5~tz9xJuMQ8T%iPUX*E50^;AwN~FUd&YA7 z&mSKgw`nv#>4=8^Zn!%6!%ogZA{^{%x6zB&s{2oRUQot&lXw=6paIGVz)OOk`UzMD zro&y1gX%F*gMl@rf;75|&bfJ<*puHBz=ntb4{796Rc7%8W%2n9<2BOw<6yLsayRtXJDMp}+IY>L-H(2i zRC9mZ{tR>cZx&z|iA$|4ph}N7?=FCPD4nuuq>#BJ^!PCJK+Cm63ts17WN`*(v4lrE zPi%01r)&d@wt@P{-$dLO8b82yw+pD+d-0|V=oeBS|9;YFHP$T_AT&d~b$;6`##+5! zsZebv4?uwXlVy54`5iiZQK-XcXoENZ&h$Z@!ogII*28rMmH`0?_^1ztwr=y29 zrz38~6QJkVKu8b+kTmferjWr+BoLdAQ|mzgi-70*MSE*6kBDTrc8j9fimQy9H8IoT!F-|rsmw7Y-Mn=!QIhcw7^)2F|c z>YC)iNJZs}01u^BK}gOO7a-qhTwx1O)S!R2^LX8cZ<#L-#Q+{mWjm^UvRb1MzF_h? zhrnN}MV!-W5UnqypEYQ)il7DDBt9G;>eqtOdBs9SwTX=G1Ao}~E}VA53N&nKGJdEA34T(bqUor`9P%os zk7)g18f=+i~ocC>RZ;<1G;k|PnyeJ#zqvD^J_=@B*p%Gzje6^I-T6el4<9th? zKP?Is{;r_j`?^Ui)$zRlddJ~Zq5IiZuXCW!ISu8D%`(I@8>2=rYprRvLDH%C#s+FG zIGU@)UG@7tO1R!uLC3k2Ca3k!M+dz+L-M*~ZvMSzx(p6r>+edzNI{XwlQX@B)YrI) z6RrLErVfX|%xZ5(U!mrEr{}rCkHW&j;TZ7!Wy(shUZ@~6YYDw+a^Py4XGmniXg9ew z2RYiKY-S~Eko(1~oQNkHAAHF6^y6r>Gs4wQv3eG_`29cCPZ+tP5vDON>CK9vL0s0e&rhAF$QA+t+gC*F021KoTASai z3|?s?iYS9dPQ~eM!YIyi>1PKFhESWlf%V59j>Ms`<9;pUzTH&H4W7O>NH#0@jf@^; zy>PKVH|?cM}LdY*S= z#qyoR-c^b+RTKU`7fd!0*qh{%_}P^k1WQyo)A?ey@z>?3>y0{Ass;!77c`ejPu)Tp zZk-OtWcoAjF^{|ZLbE)|(Dj-|_j@kUs^ot&G}ayeCx%AamCx%j7&FY&U{941)a0rq z!Y!Q&!W>a4^=paHDtk-Jw~$h5T+Zza!6qb^R1+)$eyAfPt$~Hd={C?(c)97s=S`O<1I}ZjfyXsHb|N0ttb%e#Vwf3JColY(t&vKs2BNt>?YqT zfT+^nNg26b<()xn(t1Y_`Y5vmQ_PjW#+dnptu%ntKR3FEN-n9a;?v0MLoCqKr{3s* zDvEp>l6c~tZH?{i^wpI9b8OOPH`Jy;7r=ZK=dMyu$(G*S69!8TCyeLlclJzm%#c!> zaPX!8DP68`MCdSH<43n(>#BJ!I$UjOB;uaU(DIfsSx5B z(DhGFbsJ%%D8fZLVhg4WhM0tYvtGT|E+8R8gP44OQB(+aCHD6wF=kD*D5z+2`QW0G ziVC)TNB%8?K^mDac%a)9I#NqHndq-nd zXhbeHdOhN)l;AJW$p#7#ixQ&6w=*$xx#cYTgMIFm1_8Z2i*b{#=jyY?Pe*?059%6U z%@B^c95mpgH8Bx4CUUl7c+7amlD{g#$Kx;?ZQPV43G@kxck+2}hb<)_l)Pe7DMA6f zPPcwXlntd65t%YB2FnrhWq;d{4#wa_Xdr0HuI*IS;6Pr2>-YcB=f7*^CNYs}KAo4e z#IIAT(lpgt8zWrh5*u>bCC&F*+p zXin~P@9nr1v(bk8_OM>xWmsQ`W;VDw8jSKK++ohA;Sc{~Oq z9SnZEtw6)gEk=^q6sG&t*AlxdJ4FA3gNOZW1-nWQB&IeoLSxu>&x(h_?d>U)3dc6F zZHd^BmPUG<@P$UeM&?-7K%LV8+ZHQ*s2b>inry}4 zjzv*M=fx$($`!J~j-S&*l)K9%X6gH1eQdW%?>^R9E6Tt_mnhj#lQl_-OGu|lVv0w_ zm&_Q9|0bm=OJ;iDaSLF~;_IGcEkz?bTk906GoMOG4#wd8iALvW7Cu7_?C^d;+dNBs z^?Z9Jpo~0KGbEF`T~@DlXnnMpn{LMWQqH*KdJ?p2-O4$K+jy-{4WuIj8gKS}6_*Zh zmrD-!*J><|F8g3MKGUIwk`A8qv`7jTBE@0^xF8MX=pdh6HuU8qwGbAxdc5Gmu-TU9 z;>fJD>A=HZ5Dw{9qqIZ;3G;256*xS$nND>&OLgmzkIf01y1p~#oll4|VwA|kS3JIi zJ_}#yA6Sn*N_4To%Yznn+p5(d;gcQCnMFa+gMSox0Xt!)-AbK<>h%vOibXP2scY3P z)Rj6N#V3;8B;*R)cPs)n!0oV?mIOp*?`;S&sTkZNS9~>-*T-5!wu&d`*XV4zl%K|8 zek9}@8f7sot_bPxR8nF6XzstE8x83&iU?ph*|1_)tqbWl%~t%o(^bKu%n#8_3TPF> zPIj9DGos?1?`TAXRidiK4p#1+uR<^ZW#;4Ame_5cQJ7}&wg$GpnCu1{6X{>2I?kqvTttDF%nds^dPZHwzoRZnh_ulL$!B+4~LI9>M2j^W(@|Nzh|P3k>(#EQt&1Yt3i`qKVBIZd|@slCghq5F%pK&*JmO zU^W`;4sgbsBeiRp?m#-Ku{&Nnq(ntYz-9B(Dp!a6IT8^#|J*#{Drm#{s@oy?M=ccA zs1fce&esSf8*CTu=5(#_kKw6vFU2O~NGRvN_EJzi`Mg<7M9S>X)c;`lbHNDQrs$zJ zl@4%(FUqA)s8A<3_yXhQq^gu8?07g=LLEu4RD`_eAbytx6&5r@8I>#c6bQaMtw2bi z;f34uC16%G#-LM#%A;hQ!~#TsQIx1kw0>#blwl5@q|L`K#wW`ufYfa$(|!hppG3@r z!a|Z$2yTMt!#tjskd}g0x7l?z4OWXcII2z19k8fFD@h0UOIGN)8Fz9q7@T zDB%@F1Els!F;%W1B~;SBibbe)3eLhL^`W|N<$fIL)&jc3x#tare?%3$)U0c>qo+1a zZ`%Lri7{>ZE4LzaQ{b$Q`2#!$8DuDIP`YXmUOY7{p%xYZ_m6i-z*nPgE;5Nk#qkvU z;vzZx$bQEn7*v4Y$H+GcAbT!f!#DMiQ?Ec6aG8K~S|Vd=$ZrCVJm*1O$lpH(z8&06 zV=|kL2<~*XdHjLdnFYCzK!M!0!+up98O-ha)1WD*H@)38eiZ?YWpcrU?a7FY-ttV= zshI7^5jWCoW>F;gFn`h3}AT=I}LG&tuZu<;J2jukzS0O*3 zNtBERPaQ(C3&rk!+#QKX%~nnG)%);uXYYjv3YN`QnZ)S$@J3)&V{$GMB_Dq40ROUNWSqT}c1j1pl8WVzw6!YEezSx~b6*C^`9>m=w z_Q}}{KE;~M`X~9zZd`O~tJj(9yj4mQZ@E3BFpDHOYd&S){{+$n`IM3n%A_~rFFNeb zeYU1P$Co0@dq;xw#p83KLzXXw?jU6e`0@oa8FV@U7Skqh^2c*!*i;E?A4;ad`ZtNa zhwP~IXc8rB(e?|SuH1>g(gK@D-)tr8onD>V+^!JzWV5wsqH`P!C6m9k~>67tkx&rn7xHzG~tHWO<_g7%lKm|}~hD@ySf zT6r3*HGAbNTw3Z0BtcH4in^TWPd?A%vU@*PDBdeZwP@L3CR$llQ(~w@Gyv9N<%)8d z8=0&PZt1)Qe&GuiVZ_fLESE7&Oc_5WSx=cZH^xV?_q8ixL&eD$zYI#nb2u5Rr@p6C z@q;1y{XfVQegO1m_W8G=wL7M%Ro43vAp!ta!B>>HaA|<*l8XoPs)-mt~Z3pq*F2S)XI?vFcbf-_g}ow zVRml~<%Pbj5)q@$XxMyUs+NYKC*f*mklA7zO?vx$W!#|x7sVAC(@u_vk%#`>rhpB8 z%TU=EpAU(|h_d;L4@zu-gcTj^KV`EZ3jT}r$(bW2|2(i=a>+J-Pw?NYSmjbX`2S+< zJENM~zI_Qs1Be7dQ3OH@y-U|nq!;O35Rl#lq)Rm@9qGNJAiYZO9Ym@E0#ZWn5I{P- zk$CPo|8w8H@6#J&e<7Q__FQY0HP@WKIXAak+aD;{keolVo!s)*b&uUZdvBDL;&0iv zB8-4>oTSR2aQpuV&AOrgk$~wNZlm2mdl0f6_dZ?_B|B!L6Jq&a(3Q$wMj9Vc7AQvv z6Us-pKp3Xrfwmj*4<;3>8$qR9%5x{nvHWbA4T$Pd)jrBnzZRYB0 z^P}~b7ED#(Q#ugt1b_bDgx!dWs*e9&dGYV631vBdNydN?qAZWk<1$3skuE=78V;KKWwD>)7mPeKcxAzxrCiGK* z;^LVeQ)7FRy$?;*KM|dSZ%DvBgyx3m7&i_Tx)3L5q=W{;(1079bz-ov1_OV}kK`Hf zQ9`!INLVmArVzvq$P8tM4%Q^;65&Sqza}1Gf`(@kGo`TM<*B^;c4IiEAC7#=MQ5=u z`&SXnf1!uWB(Y(}Xp0;QqEE!H!ql!fVCMw~IdXCalk?*l7#LK?56qK(jk|Lkwk&fz z`uZ4|hk5TMQF%{$P@8;PG_I~Gm}feMzKkX^0xjRy^v2;H!H@&NYAzk1^rMe5f%}Ut zkD4|wfFk$pN4_RvD9Q3zct``NBFgeu_=fmNM$ASBfH*o}_XGzee8S6e-W?S3+980p zctkwE{n~gY`D-u56^QM9)EjrH*>9w=9(b)p;q@)#2crb>`cSYBw#Nh zo?G3wCb4jnBCya{_kYoI&*%BezQP@$H@&h?^(Px|H*oEDH0N`D_e&g%Az-NT%9^kI z=9R4uUzhH}CjlGWLSvjJJglNc!4|2U+EQp_OO0aNgM;!Wed59SHEZ6+!JJRBD1DgT!9YMX_7augcKJAJ5{+kvwd@hKL1v)73qBs~W zEq00i707l2`fLyG`|@j_dml!dq#96UlkI>aW!qcR1lU_b-}zm{Qjc?y~V~inS-qRM7*kS$9qx!KXFvyN%C+h z=ZJG&r|00Hv$s|?q&=Z>{=m0{r~AKmhM8(#)d{6~^u;nJJ4gBMZ!;_pzlSNUPeh^A z;SVHYJo$jdPW!a>5JxlYi783tmo-uc3w<+zru2YAh@zq-b!Xrnsjv{=t9E8v3np{9$2{?>F)aL17x;sX9v zb8;7gha2S|jj?^<29E}nv0;SIu}Q$KI0}6e*ie?}IS~M|LAtvVaZ@;%|I4&jmDXV1 z_tK8gCKQC=<{9_HFv#vN28Guw_pt?=(gPseDOlaN2sxZ#_$4|-*2LYNA2P+_qt+`% z?oL9_vlY9MebKDAUL4c@`LE_SRunNZb4qUf-IWXKYZU%!g2-du&Ss2&Sa+73yYh7U&V3$(>M8DFDJv0SE=9 za3tIFRM0_a0G&9haICN=f*k{$`4u~Z=9grm@TP040#dLP2eM;k zu8~KmU?v}AJp)F5|8}7}URD1{-|Gl|Hd>nFCzPNmQaQ=G{y5i;(i%TXz{%%b)bcLig^S+Wvg8rpM2$&kNA&WrIAkjz*uLzdng7AS2qQ# zyOXPf)8A{2*0S(pFtJGC`4il4UcZ0eqHOiOFLo~tClqw+DNib$@jXkG97c5h7Gt0& zJ#Ulq2q;O#FNAtupYKezFGhj|GMfB*%jWL%O_!kdn3EZ(c27lQYtOb0c6!0-TKxfL zF0nR!3mI6wrN7$uT^l8~&ELat_uX>~S%)tBjvJkpH|^MVG~iJ4>Yyt`2;Z@ z8ffpIpUd2h@brA_WlsW*6z!zU{Dj_!)!@2bWIG>yZ;*@aRf(sUJ2$I1oQ`TnP*a(R z92t5ElekF-^SdSTCac_^%#~o_8}GmL4!v9yV{(aX@@lcOGJGUv*mU7f@LD_QRKdOr zXOZWNIR`9~O7e$!u}ARYLvqPYZK;$%C#|y}F2l~fWBtohT>gyZ1CxT*oBgx9?@ND$ zgq=X2`_vROK=Zy7e%k&KRPxLEm}goaK>}{IC>C!Z@whp=%XwML)fHIauxJnrWq=U8 zX!%50cu$@L{9e@rdJkYr=`EKEq)h(fl+l;b(D$qw!Clf@p@P(6_tYP&u?xB|>i~ah zFsjuIapZIL7M)OkIU8$aYFqEo`r&s|;Xz+|DvGNb^k(}((pV*#6bZgT{XI(uF`SAZ z4nFzcGaCjzUd&vFmi>ECzCsV|(lU2ndhxk=jp z?vAu`QQ0ijTB%wsNlcf4^|M$T!hnjEYC(%#IHk!R->Smg3H9Q)UKQpz(Rei^t$HAi z?s!rCBlj3)o=NEWhTSl~x)l5N6c!P^RNKDmlUuWS4+4glizf)dsBYb7tzF9Lg5dM~ zP$VE}s_D)y-1EJ>KA71U?d_Gk{W;2< z$fujpTrntf)cThv4~Y6KpAu|2tHyWUL{{l=yIwr~;c|TWbEHYi_Z05;Zs>3V`L%Yh z-A&$sarr%6slKL5N)?%s@33vPM-&KhBIJRY3VE-P)nxph5RSxI>}=;X5+G5|R7f_!jeRq0*s*o$N!j4cCQeKn5(L#! zSU4H9j^~oPS<$bym&UnIKhZW**2<>xGT8yl6tx5I!kM2fBA?0GY2)|#g6){H3_a+?@RWs zU8}+P)otO>2qvzaSI1*AVIXT``!cfNOMT$ruh-AIKMD%O4@VR2e7&z$%c$d>A@2RO zYwn50n3Ut|i14is89DP~N`MY6nJ`LRdJ0XM0iaWy{N$G)6~eRH`MBgDZZh5v{WHEk z-{MKqJgn5=TsEYG0gb*~`@Jy{1HTPoW%zewO$;NukD^9$0y6=Qkh#&i>Ie6 zY}W=Yw|AhrOJmLiWPoBHHk#tWm=-W%*;di!4b>^)@g)3q8-olAo$l>`G z*=ebR?*XpIqgAwS(gfXpf~BEFEh=&IfwQ-hSfMG`8DRdqW;#3aae2Si$`&tQ7$I8f zm_mq&HS~X|8yyFg{VI6o%g18#F?iFBLHcq^*AMCKA5_VwmajZp7o@i3ZaV|@v#o*JpFu6wpZXHZ;fTKE(c7jGe>l%J%Ponyk&)Hd3yGKg@Q{VYw z|!`%}Pr@r?*HChM47zmiIi!^Kz#0rE+X{3xAl*BGY~jfEPd zU~_KVpI{+IC5JVHCH7Rn3JC5-I9?ww&-J$&!s95+RUpPeuJ2NY;(2iXZRSrc`v$Twix1PHDnz zbfteiIMQp7=lPwvthkpoy>hU8tY4i}V_Hb>HP3}T(O`h&F;@scUELf57TF- zgC+%4_Nh0sSwTo8sU z4yKe(O7({{%2 ztbi89;pp>o>N`A&FxVL8Ec0YXx6H8}RU8O@wh)5Mlr5nEYD;3UsmmmS>#h41) zLiVTqNGQORl0QstFX%O8@uyno!xjiJ6E3caI!@*wA16}cW4tk1;9q>9GVhf|$9<+k z>TDr(;Vivya-QxyhMxvAC=8?^WwNQ?sdKp@61aKOpTzktsn)HXl28L5w|&}ef;HY< zx;Aeh=&8iVjJvC=Ph(~2l;xLMB9arNVmBZAr9ZtI4+eRVjN%#k)E(?M?77slW5iXJ$R0!O>1_6>1;Ln*<-tuv z8KxKS&rcw#O+`pS4`4$dc#!l?+8LVq_Cp59ryXwzUa9IeOU*b9Jv7hcl27x=uPx0r zEKF}%BCJWTFP2slF%sb8N}f07yD?}8a7z9CE=f!g)wTJw;VT&0e$l=}csTF|x49@D zZ$XheThEo0R$;Cn>Vy@W=>VYjD>wOkYl2s&ko)t)OrWPm&SIDJ)9v<^W{?wJ;u=iU z@8Z}aLk2DX^9zp^XxAwpc5hWW6NFGJ z2v2~3Qw)P~$LAz_fA~!l8P3_X5Rrg;N^Y$ZP+(68od$mEn_|V)4f^Aex~`6`>hlD7 z5K;p|e$&?iA<5Eo)u$>Dc`M@`=K+{P;JZK)eps?e%|{mcT2jc0d|DJjk?QMx>3T}? zIO>2?XSuQ247t7^*|u&#mgg`@o6AK#yy1Bt@z{RF?|YCASG`H2(WrCIneecaYrp-( z`J(0iw-ZQWr@id+U#%X5#PmG;F`D##0xL}GVruIC!LGFMB3SO{tutJ5sp7_a0F|utmdmDwNy3Wy0>{La2e2U zx5Q6rGA2MO;~$XgxMG`EI&YB22<6n(ldU_@fF5sof`Jrw%A<%el7py6zLb4C0hG(b za|deCLCevCH}X=El*P7WgV{NZA(?Ma+2kOyo8BBYPW*i{ z_wL>Z2Yl%rvi93D$WFrRWzADW5g=9nwJW`2K|bA#NsY|VQvA&IJEqGBxsmGb=;WlP7P0&dj|Y5? z+G%v&p7Z^ElcyQe>LLJkDO7#7a$oH3{8||yTMFAFCdH2qA zn3T|`q~i@;s>LDt9yUT}7Wtwvm$G=};E8;DGdIWT3l04V_0cq`Q@M35c?jL_GA~lC zsabGl>t{Jt)Oj^da=1J(3sE(4-Uc)VIbDndMeD^2c+xHB_-D zAmEglph5Dn9QBPHvP`^CP#C?%Eejgs2*d~ShJIGLHVa(crEKX?P-?pGmK!@C@+m>7 z#`6hv&`brziAk&bKDgJK;Mk-E23B=_QBHu8hJQ3G^oV-=D@RIO5KynqAU#%Bp?Rt$ zEbVuCbQ_$}+VQTjWyWf92sQOl&I*Fbb*6z(ueG>rHs zj;;pWDlKnDpv3+_C*SkXxQ3x4lpUS!cBYi%B8_fO42<|KNLdEvYJoLzK(WG&*4D@}4sD z^38Sgx+Gj|g2ZTbAN;PREr~54 z|EJ`cF+U@n4gkSn!=4}kPf*f#BlxYWI5co|mljNI=5-^1+kX)vQVjeZ4U}uMLHDo< zOR-Q~UD;UwqPMiLw2m(-5=Q05xIOyu`CdIeN_9i%0vw$t7H)T3)@7gRn zzRL_vdlTVw1QW-8$qdba`$P_Bw$d2RP6gAIW$JT}+V!JX>*v4fKFEIGdZeGPM}#{? z2*4l!U6RhvH$^go#B_@1cOm`!o5Nk!A_jI=4nIjDGyn<)<8P^;_crO~xkdiJ=+FW1 z3rxJyIFl{0nGx^=-Nt3EKlf*hr)MRmWEv*~9k9IRj)NQJACIdFPr8q;&-v?SmIqFN zIAbVimk>e&U_;~Cxg0}57%nvZrta>!)vNkN7vMq0wZ7iDaw^xh4fHq^KQR6+-Hj*j z50t>Xj8x*l*f1l~FqNM`G;%0v>9qqoJ{(@N{P%*jjdn}wD%IVz-B0OLnJZf7rqVuq zS+w(fX~DF})Us8sjq=ap4f1FdBY<*kFL6}ix)!V~Q1;Frqc&f9+IvTnoZzs9%s*Cb zk`kS2I;CIujBHO}Ocn&!@KMzKxNl48ZD-rHeGfM&@sE6Yu7!U=V^4+AKw}k}Zo~@t zjP2bCq0=cgir=X~d|Gs5EL{Tr06WzalXepOI}R^Romjm}?^MIrNb=X{p`e)*@bC9P z7j%lJ0fYaiLhDd1J2RBMcVf;q&!hZ=-0UVjz+W;9WSJx$iIyl|-2K&n(S&XHpVX=pK%k99?N3-BlU6YkaJRE~~EwzK3VBdTctw(x4X1LJmFvma>3nGk|W zXzfP%1DT<9baFHP9pVXT3g}e+Z&pqv=r66vUcoMM( z>PT4B@A|EWlPSHzvK-kCxEmpTQ0dG|^qN3PIpcd=DMD%wy zM-`rA>A(UV>>Smzspx6%)zfuO8~V#Q3Qv;BDq+J2@zKVg^WI-d|G%1Ev{hkv`hP3B zhgCGPy?*k6EV^mRf$=E6_<^xn6M>F!H=7yNk@MK>aMn7elk2R+P7GT}RD-~v&W2RDU6DCjW9 zC3Kn@nr4zBEO+;>UI}Dr;@aqxnjc!no>(?{a-;XOlKG=|o9{MW?xgI$vzE=YiyWo{ z7Spz|K-nL;7RLeQ+6>W#(S4N}I{0;b)V65!4Rh(-Vf)*FU(tfGLAl_>C3-*=F(yg= zbZ5N&oxCGI*R#WDq1gQjLR=vRfNf}yhGp}?^3T_1{ZV)NV_`MSdup+31?L(TU#&;o zuyvFVjJ0OneodvGP^)Hn3i99C)2iC`V&8`229jd3;^oiZ*d+qf68!nQ5&X0oaNSt9 zsan_5F;b8D`m)C{!sgB-18Jh$Pd?>?QY2M=nB3;h@zF@#<9Z^8T_cA{6_ZUeTu5OP zD4}PdrG6s{zg7)@klf-nK3oC5nL1JtnDGc3trl%pp^5KJa%%S+nodjsfhyb1yN4!itj zvA(RE5_>6RBIWueS=9Ei^YQL27fRk2XTtjv?W=h~RF(9Y5#6c{erlon{k^1xmd-Z> zmAoOK(~FV%tXheaBNb`pmP7)!khOVD`a89eq^foszrv`5ExtQA;hufz-WctHXVkI< z0@=eDP@w_KLSQyC!8@4EEbyHN+DIgn&AcBrYH9n%@vzcD4(%8w9W&CgcXtkFimAc; zICq{wMV(}>>Lp7Cv@>4x+H(4!V8Pe(FB`9xf7TtI$_FE!kqdIB#IspH3jWQ{cANQ$ z&kkAcR)g~i0DA5Iq|)ICEB9*^I+(Nn$3Te5`2&le(Xe+R@DG;qhjm+B!O&CDiAdc* zsloMgrDO%NZ$ZEVq}A6QQAd20%w~M?-Hc($y1KeRy=n$Fm0S&hljXxQt`pi%z-S4} zN)jPF^}L&F_vc29?N`2b*T6ZxB)ktRa^fJ2-FLvp?9DiA2U+BKl&Bk#wTKJR4?mOw zX$uUGQ2>c%PW@Vbf1K#Y%UiA4NZPcpyB+Udx7Y58;aiJ#!I9ioTdRvVv?kz8p`roc zbtu(lPg*(l0qIE61!a>L)hjU{{Ah^*A*a!51Ke2o+btY1h|mfapU{uNyAR*(2|c(Q zc_!b6g<<3!_Sg~25XgaGTz>+zUY$uA)ll4WXmpiF*z9~;6`-`FL+66*GzmBDER(euzB{hl-=^E*Vfo=x7(>BJ} zhQ0D>eP@LLmoS%j{R#()thH5I*#Vf9COdeHF|zFULUTS>#dVkGcC(|>E9$pj+pH8B zb9o!GnlIoFM2yMMa25%S+979h9N%lMtClk7$eyiav4B=5_rw-!_ZJE2ZAkj=%gIkd z&tHB&|K)MMnm#02FBAHip!qgjy>Q+S+v8%9Sk(J0R97vRX6zMF^~$oNs6{8#`@HbN zaTogJ&bc-hg5B@*<~)Jhya)LE~!<27s=e|5*$ap{g(b%ICKnc-*&%j&jhXhc5e!x7%K zeiKDxd)oKoGq{;T3JyJmwl+0E6E(bJ?bUoVr;~SA$ZDwYn@E!-`SA(dZiKKU&vFB7 zTD_w5YT@vxekfcX+1(xxLT~qII_8+Uf&t=ahmjMq7mJ^&JhNVRBRB8ndT$tdp7L>u zd4yAot*nEk!YGN}@qUy+(A5*Mr4&7Gb5Cz27@lhZk8ly)SxWIJ?7R*dHYz_~*V}IP zpyIlsY9j8945<9c+s138%^;^dTNjpX`&Q<5wD(e~mxKdoxyGbFAXdn2CsNx=Dx477 zYN;I5@a{EThCW}@^*(O;YAMfp6g924(BRgHIMsUskNK|R7qa`SOZix){RtgZB1Lhk zOCoMpobSu-2|I_>Gxa@3qBE4kN_tRati{sjlOI^5q=Ru#jmGMlpYBizK^rvOX@n4slPaHb zkfUU~lUGxCTbR;LcqbzH2^ch!;JrirqZ|*??cCXY@nOV$BM-~-NgUT(l<*_uul(Wl zDwo&>A{@?;f|0iAmG8qS^P90NSwN+-jp3~V;-2p2e6A7qja8bUH%b;nSm;bbAkVKj z!Ut7OWro_q8VmuHQKG(QpDcpw~Rjc@ATarPjr~q@Jklf zawZu{qqTe7p)KzZELOm%gvs4fjdwYljN$|v>8qtb<^+I~#&=GD55_;HoS%JQNR87& zz=XSox2D9CAxg8A)Nk<1BZi`204V&;r>^%mae_+D4D8Euk`BZY5L$KR?W!<~XAzP* z4vT~K)Xl}A<_ZWyEk$H|Ric&P8`t2o6>WMU;u2)?Se9nZkz6_rXO7hQy*)9X_HVK( z6Pn+HF=Ex0Zusw8|Vg`IuAm zg~{EM4!6GgoD8()N+q}qaJ@Wjc#)mMX4JLvwJu4iSj<;c)AhEFF2Mqlk0WyN$&g|D zwW8qn#h+11e}%}No%y1H31Fj^2?6?Ny#fbP%ky-`eVaSe>zc?;o@Q73m8YV{E7}H^ zTw`(UJ*y0NKhI#-)2?${MieS0dJxVN6FJB)^>lRjDI4!pyq zKq#m&{qgt{PG1mWE3S8PZ~mrtPVc>}YxT?1_7|a&`w7U>_OM1%>gx-`F=Z1`1NkG(wZpm9=^@B=;Zd>Pz!|* zzF!8AJH0BO#a^~FIK=6bE-P&1+_|`(R7icXe-dtcQLWAM03yc$_)#NQ36#7uR9O|C zu&htLKwu44yEsMs3 zA9u}kzSf)RB|3$Y2|vpFQWA8MDOzw@`D~yS-e=cgRm)n~Qfa)GyhNS6*njw&>*sJU2u#w6KKYCgqWle==J>zzen!OjT%tBs<_OaskHoMa#C$S*WIAHe^GFVQ!*Zii@I#=y$AEt^Y2lSNO?kt4BFLJ_no9#k zYyQke@2(hwVt5F}$$5zV$UvgGVv*MuO>c@zVWZmtipW99g=XpH#RI=d`mQjF87A3; zg>Slf&hndUBLa%&c_pY#`yZ@4;DkKZ`5OYDE7eN7df^|ea-@N)!z!0l+3n;W_<~dZ=>>5%!Ljm60rKSjA9Pd%WPV|36L9SJRSwND8^xZzc$Ty z{*a-A_LYPA+UKy7BXBQo0w|ed|HZ_S!R;tp{q={hqra@viLE(^`JdR1TdI=`pVQ|^ zg$6QI@)G(nRgDBdMb2I z1ZHJbpv#Uzim`*5>kI=3D6OwD8nQP*ztFOWGqEQYy2)UmR%P~bi_}gNveM3|OVv2C z&;$R@kw)=NflRfL#x-NBQO0N{@A$~?gvX_utO{z7aNS4Yd4uB4wHNtP>enBjq3gC@ zt!J+HB$l1Ha4SW>+f8l^c^{@Mk>JoLdcxl`#N(+S!jm*J>dut=p7{P}`kj3B|H1+g z7yY`Z(Y|#u7dEjtaH~~O^ciVRKY}ug>3}%KEYET(l!LJ6I###9Ih<-4wvHKdW6W^FnutCS@#9@jd$kRt(y|W04rv?#D z84p{B+Y`&?zq1Tgc1Cyc$*~6-jx9e$rJwIXIcGhC6F%K7D0v}06%L+@g-;K5mXqph z`MxGzp|$diN)C!+i8<#h)nJg~CG2spzgVDEPHzmSsP&#HaH){!%Y0H)uSACQZjT1x zPJAHZFPzr?nYLF`pr8(wm~~;tbd4Nj6!*-6C;if6|8{B{AHy6=ZyKd&68<&LUc_8J zGC1f!--RgZ-HV4s_C`0tj*ze5^wAOpV-$@g$>qf7!Uo^{%5Oq%o~&04iy34l8I01V60cocmi}U@t}36ZHfnNjSy;Mm`D|v$ATnt9x^+B8J2FnJ z&cTy`mrgx~vF7t9<(+%%eb2GGK0W(+u9d)GjiL#13K8$$yEDNFnhf9gE|82!bU;<{ z#?`!uE4VsCc!>@mu2!Bl=CZ$>_8Ae}Z+=-4>J}Ektpkne2}7Jqqp}u;t$!rLo_k#) zNeN!mf5JX7x3zdpywYxV@Sj>jH(P=E1y6ZNWXh2LRo?+M|VQ4Q?mrdgiB3bS#_t#^4*W^x6*mUBl~I8K^AT;7km%{}@LV$_$>^Qespn z)Z)&64x!(eI)0*Cy%)Gx{^%?nKj(Fxs~X~pFT~G(a;tXzOWTX9$r~CET_4;ung4@R zVmehhggS~m_I)?1@YcA)8!xv05jP?{Jl*bDJ}i5c zb!xo<2YC!EsI#Be3H3_YOnI)$MqYX0y2OcJDwITxC#G{6)^BWU(Rgk{DCT)6x()YJ zx-x_n%713qx@w&gOb6Xn`J>jtCRjFJl9t`Ea%Jdgme!45*bB)*Jb#>X2361*#nZa?O)Dr<=MAjMC zZ7tO@Sn|DnuPd1={W~p{hDI^?+Px(+?|QimQ;TB4dstJx z!M1{e^lQ+$^x@~n3iXd1{myoqADeN!+)g^Bb$g~&!!Gi$%0kxPXQxq%Go(_OR{}eQ zYxcOUYI5&8tzG4^!oZPVaB3Vw&{x+I0|}AVzR1akGxPc5_4H{P?wCxhbOI>9u9@-mHhje95#iW zcq{J^hB@$cBIFt-7A=Au2O2;edub3dd^)9tvaa^OZ4%3S+ti=Ha5u)x^DX}V$%~eN zrOCY{SG+3xP;(1$3-*u2x}&+s<3_Lz`B*&YgO)H#`&SxFzmu##jzBD5E|l7&Tf8fn zX#eut8~&_BU7+v?hmicHE~h1QWi}}9uNBID1lWt93aXJ|$hgq$!rDkN^LLvVB6lR{W>|?Z^ zU#P0zcJi^mFy-=$!rC;qt);Wh0a`6uoU@bII{%0Tg=9widoPg}w(W8`331{sEaRYTcr-ZVouBV-`W`?hB%#Pb0sv)h+Y|S&ccdCD-k)qRqxoFJ@Qr(!YB(XGP z5_EMw$S$hgUwbcN!=}av!2tueS-{UnTrO6R?R+<@=LS3T06W~~UwyZQ+_h?xsl=hn z+g;@;#cnlny0^R5(D}+Vtd#&1q0Rhbt12roK816UNk^Hz-D-Zkb{UmNnJ{_pdV{vI~s#% z3>Fne=v=+jxfT0QOl|5&zlK%ILkeGb?s|#{UM#N}&&4E0i+?7@8Achicn=^xb-5gB zKwXG5-!Vq#U;KBW{x&Z^2xmbtI%oTeKj>0Yyt4bVBrQ@W5)$?|7!9U zg3tgIai=t(l-5NPx}!OFLRh`NGxu%BkVJ&8Nuq!UBR<#9XMK?+VTq`$us$J#G>Qh| zxI2c|6%mX5?$DiEk!Hy7c_av|LaP<(-4|aHARTXto)v@;I}Y^w7N)Fu!d^Amk2(^MWVy6$9!e)VtWZpRy$n# zbjpJVZVr|}B>l;(Bevko)(=*rIr<{=Clo0h;1nS%Qhr$)KBtk=Z1HtBB6JHh?x+5! zHU~_z<7^l>)eYMAiA_+oL6>%UIQ>W7PFAg5e@tpEmk~*+SiW>s!Ogm%1=!QW@rK~e zv0YH_2l)Qr1q-WIQ4t$BbvDsd_lC3Zx(Btevl4=muF8>!f8M84ZsosL^!)3+XgcY) zUk>$H!51Ht+lnSXTz=%e01!U4Amo*cK7V!m2;Oq%i`}viuT5}+q zO#-`JtbzvA?rr?L7%W^NHo%)9xWVbk z)+c^KQ6qUy{8Ay1+qpf%QY$g_sx8jnXjT@xU%&cZx*obMe7HDro$kB5xVv+s+Gs+H zPsMv%%a$Mi=&U$>Sl#pVaz!LEEe-lvoU+vml=y01qdV_vkwCP$adAygiZXP0?{fm2 zCmIVkkP3UYFusxFUHaT06D@#=)>y^U<6x8Df>XDk&&76TD(~s|L|jn9r@PtDeuq1z zR;$)(?a+cRPV6KrVpa%wOr+{8+(+Ys+MAy&419XME^N%#P+hPm3*|I|hgrCOUHrgu zs8(nYFP=F(ea0RS>ZeW;_h=3Yq-BFHUdd8dvy$GI<~_TYmnXFDrf)$5$Y((NnnLz# z=H2pbdT3z{jIMEEd9#ZD*{<^^$hKdyx-Xii#Qw#s&p9Q%j{Ug3#hX5a6T{K+;?$zg z`fMSm1()H6SM%N@EqeFFP>z>7?bX-#NK7Z|`zA!hy^D4g}{3#-f&wr=G!J3ozg|2e|UZ))>G3{=qm?9H$}S*=zGE zTow_}kr{gyg=IOpOup!8`;9dcdG3YGh&qqGY=;+A@=#H4j}PZsp|pP4CJ*>*=#;6% zD8->}ujYeWem&2zXwDOIy(EOrHkO!B;@#&ev0yb&&r=(=+wpFX56S{|_$@VEoZb`6 zH1BD%uy>h>K-ZJGwz_X!fD63_@@_%(^9t}3wTTW2BC&%pubWfA1$}mhsh^k>u_fFBJ2HV zZK~IGzpQX0dsVP(*J3S&U+?4%SA^*(i6%nRaEL_dk&_5HqWL}i5V!Q;&b_V7q6a}f z#IjqYz39M5S{!CNA73Q?U<9l&5I#iFRF~ek-%oXis`3)X#4sxO!$OGZ3}=Oc$v71J z$wS_NFL$eRWxWgVr|6}`8`3kByp{_FHPb*p~iyCL>x^R09%pI4pzIW9X|VlVkDqZ!{5RZk}~s2d=f0hc^z+KA#+0bKG= z3ikcd#FKB~)p>PJ+3dkEYGdw>VKSMa*cF;f5xGY@;V2ShTf)9qyvmHUHiN(GisHnaaKxV4#|Audv-}K?fgli z!f3$s(&%nU#@@xZK~dO{7@@|R(CIKe_QA5GL%7{fc-^cg6>gM2f;bBvs0yWQ^3fB) zz(Obh%}F4Atu7+$Jo5#=jgAeh4<5(!VGqQ-u)OY3BA)5JrqRiG%R2Wm_u^6UwD*I8 z@oTF(i_Vl!4|vKcypE`GYb|z4GPqzUV+BfBPy5!f&PQdE?%~(Hq4D_Lcy1DLeZIA? zzGz)k8K6;45VW_pJ+tH?2%!n!R-`E&=xnTgRbl$|xn$#&K(S!uY9GR89_&F3=!=~{ zVkycqf$q*W1gstp(0dIgeO{Zu!zE^Fk5^nUQ8jammgy|WK@G-^+j%;`~v7ym0^n}_gwBh`5 zKT(8nLctFn^M-7akVS0#n;ZOASS2XF+541 zYI4vGGRvHsfsjRGS#sCzp54_m_DrB$TO`J#d*UWI7YdZ4VJFUl3rTOKaDY*KDdOuP z2$p~>At7l$V+Q#)6O8lLo&Nql`r{h@EZ8qqp&d4osZ@tqxKd3*oSVy5uVV(qXB*yN zk^8Z~Qg&OV1wb4hVV;V0)zeM{%C)KC2J(11Y7Q3P$6%6J0Uz?`_VIFj)43tFiouvXfCJ`X>})Oz>A#62Ac-CmYp&ZLDvX0Ud5e+LHY zGYiRr2hQZJA<(MNgdG>O|9#q98zZMwfC0qLRHYgE561`CaiAb;JncYCv9?T`&pc7_ zydTLA#kw4e{tkh9Fgj|ehw0KRLQh9=%;WT<`-SjGPDqcwGu$}7JcXc6CL8Qpdu2oaEWtQd-FPlqOU8YRd3ZGTz?uW zXTgCI7+;|lz!H$ELUPx@hr#H zCd1U3!w4S$25w#4!K8WKW4Y@i9TaOe^w{IkbCZb1TA?LbJe)vdNmH;^78~J#8``U# zXw0Cv5JMYrUZ-{EVTHUu&(*7u8wo3NIe||zU`vG7;@`0_YC}uJK{)Wy}DAr25wwWI1KR=;6eej@BZQm5*$^iopizqnq7Zi;t%Nmn^-Jw z(4C&1o=$x<`->-k(?*zwrb6>I>gB(wa6?mJ`Zea?R6IjdQLwt)`OnL+a07L)cP}E1 zudZ`H8+MTLE0WdV-WfnBl~=0%vodISB^O|~#>{B034_%9djKI0zyM-gfQk*1;rN@d z-wfiO_>YB0Z<fAP0NM-HAW&uwuepCOOatv!(aD#>TCu zuNwO^%sG&=Ea_8b2u9OsD2WN3tVF%B4O`!oF?WTK-@5z$H@+qfIu7G_>AIMU-U3{) ztJ8aDd`k##aHZ2kffIuTk&eY{FzQBoMBo+frO?LN|Yg?7&V4|8uB6<616jRHY~ zG!oq1Jy>WgKyY^p4#6!*;}YCmg9UeY4=w?M1$PJzjr$$l&wJh@-?`uYdw=#Az1QB$ zYE{iSt5)6zE@tS-Fi#;0szH=Y5rd<_)I=eO48levw_EGI4X2hk*FaH&+x+jQ2wnxA z{FSF}#4b>)1n;sZJ#^z@TLaR~jWBoyK+GSe3LF&p*q$pH)AQT$@V|UtNe}06YT#k9r${2D1#FNS!$#bKzAopGKzE zd>ug|ieOrqTy8hd_&TA*>4b|)Dkkc?R27xtibqJDbqagfqq_s8Y*Goo=QOo?&f>Iv zRj0E1x83X{B&j3P3IL@2Z%&ZyzSiU2!6CUZKVeg%RmhM(6c!FRTQ?pIY#cT$(GJ@c zdrQFMi^rs1S~%MFyzV+%sW0&?4?_ME%mA?Pt!%L!kyjpvE=5HNfAEGx^gvmN7u*b8 z%;1ww;#8NO$aoeIkLU3HV6F;|I1M0{Y)$IwFK3)!4zq72zf^LgOT?z1%8Q*w7#Kr0C7b}fiJoZW3 zmDv4g*T<{7cqF0|{I7a3Vw**lg2ygXPs>hR&ZhfR`d|y@6**7CHXj!{s z1#q{=g9MV&{xQ^$R0M#g_(-61&mMeP@~_DK$I4p-yZd5(zLqV*%Jk|q zechi&PpZ@=eROvis4`tp*Qs1#+MX$ny?fX#&j^k)mjj%xhOneRP#RNxJ>^8kpUfEq zOV&?Yo@>wA%)Tv-A^-Sj!J|?lEt6EPj$@?oOJZQx@TzbA~`!ii)^l+JXK z6VH>^1D}cl&Fo!ohM*;~6MC`C+Q>qATL3d|^xKVdL9}utYW4rgNB#J5o zl$#ta8m!mfJl@&{4F?Yg(b26wMvG3gDa<`vYcP~p5NO&7N6n4l86BUmeOYTFUUEN( z99GE}J!IjAkx8JFdOfwPaXI=B#L@w})J^{WLBW$2m8<;X`iPGQheD8GjzLQlWh`LE z-SK+;^2>6;2QG_WF|?;%vz@O5ivZ@-adCs@l)r*I?zF)D$t>l%cwPg_pw^lcZpmM$L6q<; zLkfgkskz}J)58U~WCS`cXk}2#Ya?P9^VdlX-}nU#10D;zk(%psm+{@`xa{=S_>y@FT9ALi(={$L|I zuF;_M+%)K6#{f8nTv4G(bAElKrOuFoKA8hTV9?mYbtCBtOsh3PuvttP{u%C8n0I^L19__r3sd+<*pw1JJ#muWzr~Zh=DMGCri%YjFru34Qto-~;ixt8 zb2`Y2!&_8K1Y9%XBM@>ezP7&gA6kGjY*8{gdBZ4=i!I_Zd}<389#cvxXQMMWT25!b zbWq$R-k7}P0~%f<-fpkFei9P57$?*NNMi&XIPcXmmoM*6mOMJ`^;Z1>abqKM!V&`A zt<=aF)e0ro;JVr=WZJwh(Cj~V2gYw(S`pZ;9rKFt%hyw?IxA#9z zgCwLIh`+&%9cK{CTWuH)`;$Ib(`Vh|jZEmxiE_{>)xek$_}H8>A(TFrJl}aFt8C@q zav|5>#z_QBVJSRYt}4$emXNzQ1UM__4_!jf$a}51NaMC6u~2w^!*j^1B9~Cmk*+qF zFPgWN#$_o0xz^XkQnAD%%xpw$Yc~96+-iE;6;UA7fLyuJ@!bVZuGee)pALHjAi0Qo zA7$^NE75zY%7`i^QC%7t6NdR}oe2ENPIPdM`A;UagBh&SnU|8lI>6~_%(m2x>3B}P zN{a`fI%7Ic?RLLpq}wVMtLIl-N1weNtZ~CG+(nn&`jSQlfW=lmFbySQ5$XwndxRnE z3@seFh_C2kU;6p|jHRu}R~-a=n}e}b_CxgN%mwz^lPPl+1CE(|oFa1zF})@lbKWWg zM6%nnk~iBlDZL(hV#2Abkvn$JetcC_*0Uug`klfbZb=H@V!FLwq7D*=j2?>`&u@Gq zTAa4azo)ic6NY>XPbjI>9*QLkOB}-3I9cYi=FL@YZ2L0P+N=wv*{a>j5eOsT#Lj3s zi~`;pT-XTmd0*a#Bxic35~3fjipd0f(+R$pAqg5M8a@u73AfVj^oAu{=??0bNyyW9 zJe9Se8R04`#rd?gl#F_JbDg;a*QAqK6;jVC&y$+P(djW}kK1FHWrj~H^Zig0-BD*d zq7ai$Q~uiPw5fkJrQ*uce*ofqxWnnjaiy{b-)8sh)5BtOg!=b056cfmT7Di*nsaxP z5RA;1dX0CqnP1B?2~3v4fpK9wU2Y)D(7Zv1DFzZqrVv*r^B~bc%ArFt$7n@CMzK&g z`4UCC$K#p{G$#OHU3xCFz}=H4j=eowH{LA@lHN>Mdg=>|cj>u8IIQ7I_V$LcqwD)? zDS&J*FSWi}zyMfBaa>g8@Fe~xkcGs7$9ND&&WI2e1uYdPpWTrEhnr%8L7v%&dR0d- z3o}DnD^Eb2mrZ1vHw77%*msH(Ly{d{{ic3%x!z;@jwpyX{DhqgtdW9zssAMw2Oe^% zkMfo%XrqpH|7f#<6D7|7C&bCNovAT_yb6;BcAHi{V1oY(g5zEQH{j_5G^ob!mcA@IO>#ZU-Yuoai5FQ4q41-6%bTa}+q^3<8>P+5EFh6 z;U9gwaJ^NrDPS`pF~mD|RA^)oN&)2uB&5t}A>d37%x>@fa-0&>+xPsh8Cy^n_;scz(NU<;L-NKBV>6vzr~8vRHMX0!wyP~fx2I2`VewR(Lpfae7SBZ`nq%l#R1(Ud zp;*GO-AD^+x2%>wHAnBa1|zF|U#GUoAa1JaZdvZv`!a$*^#-}Fig6sA^R`0PU)Z;! zxcnzmr~x0IKEQ}r44d5zFOT6|S}ODf2rX|O+#_?E4t~7Jx}U00z~31;YkyDQGX4qu zG{ChSZ~Jz{ue?nT+uq!Od$T89FpNZiiwSQsrhBha)ir_l)#VuTc&UlZu-jMcC2)Y_ z%dEsx97TMa+v8rNHoqm)=~`Dfd^q+GtMXqZNAs#aytz_($A7kB2vO&${PIlj8q2dd zSSvZ&wqGB^OW`0Am&gxz+|a@;5M=0>*f`z47C7u;FXV_7o=Bq>+u^zB9Ywk(^?Ynd zX204I;;9kw5%GY0ts9l2ggXU^aF$KGb=9A<06f*KCwIKog*E+SNVKRzfYW{CllWpU zDKc^MMJV8Gx=xH%zuQwrC`bC|f`g&c!IRZ6+cT8_m7t-p->3a~SEs`}V~I#SQo?$( zp<<;R$L&K}@8^fPSWGK0#jVv+8+Y3c5s@evQS;w0^GJ*G^=^;T=)@rms?Sg4$~}$W z9t^TtoTdx+F=+u**0f{ls@<mj7*%O#KT#nQa$8C@{GE2%JeGO)SV%(mOFQrI(TH#2Q^DUjUU;i8*F;)4YXnKvOBbNaz zc)YM&c{%&=lE!7&X^{F38#J|MTRa(uB9llr^dhueV|6A#BX^oV+7tq0$r8j>8;TWO z`YK}l5ZEZ4Fzb#ekuup}qe?21SllzJ5L#<-5BJ1bN3PmZFnEPv6UU{Yn|!v_s`mF! zU5w`pTRl&oVj|M!#d)t#%Nc*(C{H#B^ONpx#IctF=9+yR9V_)q{FM%0$<70I^#Ypr zBTT9tXoHFwkp!HkrCVBT>BWbvCJpB6AX;oHnw0VRVfnF*HPVgw3L(!ca;uVC@PLcs z?r#CKS0ADn(ltnrOBXAKVu8xLsrs@ccvVYz7PKo`Y<&O}oF8Tf5d^d4q)m3Zkyw86 zQiay8P7nQ94K})@2&kz&4c%JhU~|0VMQ-+E!TTI+sZKK%4BfR{_&~5IXzEDqA8F}0 zl_-b<6^C1Z5!Q}_DM*2EXDnBLK6$LCS-m$$rPWiYLho4#1{7DwJDKBUx-eZ{qE^bW zqi_}T^OZ_AsU+eX9pI$>W}r>uwS;eiYa@kkiZVV_;dG3%!VmxJ-+4pyu28_=vbC+T zWpA4iapvYxe!o5VPtiHSZ#As%trqEH$;%!`{oCyl5BXid$RQ)dPZ{V>J2>~4D5aXq z=;EK0c!z_YfX-9Z)-s9XM%rp*hgz&f@QycI*TX5??6Nxw03FRnr&~fAcbVN{--)B- zp+Z#4S&HsE_FdK#qC^oLJiWgNd=;l|9uD5C!&=Oy!I!NB=-=FVI*d&doex67h2J_1Jv^Di)lV)-_ z99Zm(t5FO)WRW|aUj$?wRHFWU)G5*DEvgQqd`9!k&jFj;4*;2ADmOw zt!k}Soq4ncoy@u;nB$r^xxR2`gW`7SScx@1E=KU2FZT50(DB#=-ey8Z0I!cngOJcBtB#5=6txf`a-Y*;02@svhcuS;jj3W{@mP!b|Ho!k2v zmmb)Y(1DWL5{F9+cnIg_@O{w-p_ZC!ER6>}9d|j`M#O8k9$@BwinIs=)tK#x(Nw4l ztnfUgv;6ugr7fk(=cf|Q>8 z!P${7rE)zKjj+CqRv%l9^5dN4l=f2+C-*17X#f@NXo+u~&y248gK0^|_xME?R-P}v z5jzoMdhe#+P}Jl77HPXB4QOP>D|<>%{CuJQ#?P*4EUAJ}B)67-n2US+U6LqN_xZFu zS_grxIbnrB!>?-Akq22FAf?+3NZbT`ZfoM&nLBg_4X~^pr3b)|QyQ@V>fj5#LO&4* zT4n17vDG7;F30|evY`_JZ@gC4cNC^JUuVeQrH10!`AG&@56m5ZV_Aq#kJx~h7rFF} zcso!>snC`)+w)nqIK>sBYzUJd{>^N|qW4zzZB)ATeTu`M*c{(qTS(0vl=ue;xn}7o z94Vm%Bl@#!lHhrRl&3GSGbzJCrA>2)YuJ1g7XT^%c4E!`T{O(%Q1oFe?(4W@ref#j ze8u+xFa?+a+X`4td7}11-}hL+S8akLnKg?LRnuOrug2)c?CE-+p+M%A+JeP#s63T@ zs!4`t?lZRK<^2r(`%;5X0|L?!ui_uQpYC9DgnfH1#$xBr@Fp`uMz%xp_Wf19{F(h~ z)LL378{h+YnuHGK-qVB`83YfVqRpiTWFo?M3CbD;YZ3ksxhn?#%n!f%9xinFBH+RZ?@cZ`vg$JRgE<@q zbF?`uBi?4m;xae2rLqV`kP2cfxg7)%aGIE0#br-f5be06FDCeM?&qKnf1lKtxEb#5 z_P(#M(6COLC~gW!PQ!EA**yIIcxNZpQ>m}?a@2mz=$GPOFa4akAXU`RCD#Vo{LD`> z=1|57=I@Aj_Lc7fcIMCHan|)-W#w?Fpjj^n;%M-uft#NGY<7Cw-c&9H)vvt` zQl9JRg-QGFV&}rlBS|$Rw?s#)o}VWPQPel|Ch5um%o2HQiyP>&@14FRWmV zN_kG9FODD}!+bq-mBxoao2cH2a*!hL6nFPf_KGH}V}6y)^w zKpu|N)eIK&wD}ZPt9rJ+=V}ots*m5om|n~l=F!W@!?g#ZiOC@V@XtfqcxENBaf3FQ zxu2DT?AB7{JbJO4KSSM__QW2N;KR?BJ8?Td@Mug@O%@>Dt_NLRr?B3?el~j+rkl^K zErcjMFZhzpp)P+FP&<&p_&ALri&2O-yRE>Ba90c;3$nERK!56taYL>S!b((r7~j^~ zF#VnL*fajJuK1_JmlhE%$6xn4lgQff%R0u>QWut)WzI-_YI;(r+7KYmr#|D5Xj@eK zM4=m5;%4Rk;%dYcSD9JPu;`aTDg5AeuYo#yq7nl0nE%3^d^nx!=*DQ_r8R!etkv)G z2cjW(0@Fk8JHDT-Z~>c%%{M2R3#!XOqr@*!>Gh?ox{_aax|igG(3aX8?Q#)2xx^C~ zZ7{qauCVy3K5MQJy|;_}{N_{n@?9#@{p!B}+r!Jaap3TE=*?Mw^~uUCZ@W!1shS}3 zNTxRyZ~DN+Y8el62audO07v)-{dh=N+B_1A@y%2#!N(tI=r>%Q|Z#KDCK8kH^i(&+{ zg{Aerg?}0gdTNid`P72P94*70N`RPBXKU5hAjzP|k0b2&5OosDMvU3^3JN%<;auY?MpQ|DDFhdQtA0k=xUMdj);rxFmjYh8abt#o_okz(Jv6jdBLa;FDOCoqEI_VFka zxJf@U0i4MRy~paxEGpY`<*F$U=bfcjvaGg~^~3o4qi~5^!Fb>L^MQy;K=75ZW(e zzyPplLjF(uPBSTV-dtaW$B>t#)r}aC()>^ogB7;9RT1u)cp1*1ySf7FKAoJ{0EUUR9@h}qF2nB0udN00mJ+>aP;Jvjdun`^Of8xy* z41lAD#@=&WJ*S(SHyVc-!j)5|U4o~=*Q@OvUvF|$K1h8+)2>tb&o@AT1DD<2?NUA< zuyZ4U21_5CewLMwpOEVAPNa#AzwpmrqS2t}o;q!8iU3Eco`kf}!qv>pv(=%RqrkK9 zD%AG(gNAwlQNed?rV%s*yP62DL$8%%@!;HlU5p``>fZ{^)r-UBQYNc_iI;?0wI4V& z5@ep5%)wb!1{@BzD#zX4I6+66qQ`$#IK6jyJ09;)Smkg(j@S}TeH}Gf9V!C;r80pO zQo)|06$MR6?qmjV11thfpu2U7(kL8A6GKe=W3_DKB65MC&YK%tpcKA|&Q#vHDdPPQ zl{TGk@*EzSR(}-s@5#ibfQ4W;1`qzN6ijxxBvBXjH-YwXM%y(kFoilC|a{DI@--E8)h4Ig&WG-JP=f9 z!wjA!8OcV7Y`0gJMh62WPSk_Ws0t{r*N8Qv;y+$U*58xkoX4HQ)LsYr+ z0MPH^!2PiWYeCKX#jLM;^FFJ#kk5tag+<}o;28jIVcy&vd;f-{Kfqt@7|-~-Y5Rex z_KtyY_lsHT=Ho2?=Ti&-3#xxcjw8}P8@Thz*!VWC>+G5YTrVKA+{RPVk%Z(T%?0I9 zwL@W2S?6wj9_OlUn2#{c8KF3rBFf*9pb%PKlJ(-BOYjjb02U}AsFO3s$j_n+Z^7UT!n^b$BP?UHH~uU&5Q>*@GE2l&w-$eZ%h8FPfbU z01IOiI>_RN2#Mb8_A-%%Ls3wO*%S19A7!geNSHuB$VW~dCVi#95n}@onyz$~*-^~F z?!>S9Iiuz9wO93D2@fzR+R>0688!au4}?S-e6Vsmd8WZ83>gAKMGk^tF3sddpjbRa zkn}rxyutTX5zMzaMq5RlZOYIS4SEAa#XtHST}u-&8d12-On+K%O^FQ-i^x|Zya3fS_0@>J&2wJ}lRMDOT~!hscM3y5!0 zZ2NZ~V--fle>JcXtmR@`M}7Z+n*48D(PRBT@YwQX8+~PXa&mI=%p}|3kM*i#6{7!` z5E!FyAj|P@0-KocLm`Ry3Zvpb^Fb=SmUInSO%nL5|6xGm{ME2>f&3aSwRO;y3xrD< z7H@Z!RAzddOl!J^QC#Z?)#%hMGnb3a?*5ogLO9)_E>KTh4)e$g#(94sbCJXLV*Wrl*U=whedTfM~iy5PR9?VY3*ytV9bu2EvUo- zljMI$axfIJ+7|vx(;~2PyS=(f@}L?#nG~qH;{m>98T(DqwHJyDbQuytEYL#E3?y5= z#ql4z-rI>Hl9B5l5-H-HPv4@JQw@^B4O8 zaGw39{ue>h)(EdJ8cP}z5?9(ARsVfCK;n>cS2{e%=Iuh!rUKOQ968vyZ;V$Wyrk4j}h@iiE^(==` znZ^HKueLoHq*jt&_rz3?eJA?8uXPGsO?&S7tSM&uA}7Z4IA!&V$)MyvZL}s-VX0lo zT}y#Y5pb^1gAHQ%{%rZR`T@1e`#EGdp33C0l{u|#*B?%sQAx9bS&Ja@<~xVY5bdb= z-Ko2`=l>!h?!g9OB6HPKAoYVfJ%70AKkRLLvFOhvAS5Y|ultNw1Z^<@jGN>xdLi}T z;-KJ!qfs{}kK;NP36%ANN*y8HLS) zEee6(&?u@Qm#qbyLBQ{~kXNe;y-(h#TP`ef;(YE-BP;YS>!>;`S*0Ps#13*#DKHL6l$4H>{P1JSy!E#>620 z9xD$=gLw+g{oSmZxeS1aAjSQ`M#NPtQ0*j1ICQS0pW|I28f1uancg0#`5nTpjnhB~ zth7aqe>4SwbfO?5090061I;)?e(cOrl7^!(!iP&jYB$5wd25!tC~fmLhl{ z9!kIlRN5Z6?Ea*Gd$P=zwbTu;#5f(A7)w1ryyy0Qxx?Z#8TgoxR2nDXPWG-^AALlv zxM}n`t1^zv)w<{Cf*LJD7*%9{7&rH!?`)?_h~LME{N?eM>$8wwOtoPb#VJ69;#x_B z`jx$KL$?V?B^7gQ8%uWlK(?B`*1TjA2Org|GV6bwBvqpamu~IY30rewq#ZaLd{rT0Vc!$^XTa_PDQVH+a zhO_9|Wi`tK-dX)78U;OrX+xT=0v6Z}jn2ng4}J5}831`QIk(Vz03R_S4$jAw8KC|8 zU_;)Ui;a1|vI^;VD#}v;3MQqAM2GiYC1P*F&iY(tqiCh3ow&D>B{KyB$m8o7+Y1p8 z=JvJy-p>~Wn?iMG*yH7@G{CTo+^?_NbcoUk{4Ve2E|M;0DE>r`(`DH{bAgfuTw^!D z#q#|FO7NJL#AENzr`hY1qlGjfYyMwk!he_9sU<{@Jua|>EYLc9Up~rwqakcqu$m#m zJ>)~X(os5DkG$U=+`5j$?^=+EEP8k;jwQc%7;w3}yDM2Aio!v%LqhwMZ9*%bLVG{Y zen(78%9pM!jb7Qz}U;Aq#npDzpfD zePC?mSZZnWVFHocX-EY6%EzyUmhJo;kZ~gW)L;|bTy03HQnypGT0{!QV>aHnJ=jJf zS?oeT!h4Fg%vQpre*q9Q8r2dpgnS}eYoe8KBhev~FbQq24&lGpGCfYfwZ4&)vTC`;$_!~!twG!D7Bv1D_L$5hgk`2vg^&7rdB6U z)d|R8)!p7F5y0Y9rAgL;-&%?P`KRXGVWk};E_0nr=yGnFQUN=v%Wq8L)?TgI&HlOd zEz_*K_~@0$1J<_*xmT4fx>DU4ypoxw&dp1OSC;S2|9 z3X}e@-l`uz^uNfGnN2YUt+X#xmj;-;GS1)k*Yk_2un@6-z6kIv1-GmemzkCoL>2Np+q=a*s~;s{W*yzbM-N_ z7Wi#+0`}@?Y1Gk1!Hlgs4Oe$=(+BFF$wRFZ8tLQB9J98d1FyY)TNrzeFr^B3he0=w zi;2hfo67g4&Ar0NzXHBg&Uj1}RRXn^d@n>?qMa(F^&YMEz@Xuf3iek3>04XL zw8^JGCkd4LIb;cVP)zZhsUcmfnG;rrr-~U@KQzYiOTLxYK8ShFePibAv0SX@9*lI&2`y% z8jyZg3!!)&)(1L~47s04^s1WAe(>DhdETk@)bl;qaqlK-C@~@h(ISs zNGrW-)a2nwSBfXYik}avuZL;ej}IxNnMHEJM4eCtZn0V!yJDR3%+GL z@KxvJSaKIO| zZ^pI@iq=!iHb>_E>~FG<_9rQ|HmO>CPTrEOwFddB6%nrWjDGOZS&DD};`EhVkWIp% z);;Kh2Y924Xn0K5RcDFP=9geJK@k7NmKdD)klRVg7TgxCw0}pT3Xt2&7t1POyv$$4cwMvr~M{R8&*ydJ0x_;>9@&f zw{5PwhEWGApIJ4qd8% zIa?bk(H{55={d9`UdMTW+?C}6dusoGrswY#oyq{ZQW#BxiiFX zr*i{HQOYdq-`+TJNvy{Q#})|#r2+NI1Rb;lh4+LC`vR< zI_Gd4Yx^2)fbYx8s@~TsRWX_Cobm-~o3l1L|h zx#ojjPpq51B&s)`beqjVIBn9;v7%^!v}0HCWWNID32CnFjYk5l{nB7&AdDxq4;jxm zvP{iVcxCAuO`n-1-od)WBRb^B_p#yOrUw7EKj(NO5>K{CZ$iV(J_w#F_qFrz1d2Y# z7(BuE==Q0kU|+kG7pHJ*eAx&I`QAo!$|}e|@obkwB>!0L@H&W|goaWNt7dtZ-gtio zrp3jl;Fk5XTPbowPoSxVf0NNxstxUb^&m7REfu^GR@n#9OnpOuBU9?!+&n-?r3v3i(Du{n+W@-2liQ8OZ|K`? z+tjI&J)worRV`v}L>JmpesERxo5LOqTF0Y(%bfT3VkpPjjf$q!FEND4?PJPz7zqJA z;goy1iq0x?P4Ci1ih{K4$IYyLrWb*8aT5oGpTIGH;)J#3uWp~JPgm*%_4BC!BVlMO`kton9^0P z5*RUS#iv%yF)_LQ+-KvE4jk08z|g%Vf49NgC#sE>A*_c0TW$&XNM<|jn=%@t+1-P< zRYx5^jyY$uj-^4wHzRIyr;o#G2g>_rJ1qGkQx~KkSwY-SjFSjpl-xlzKY9RLjxU*=`L@p z#_}jSK#qvd$8yyHbxjoDM3$g~e{oV;7_FK@b!Up}W8IFp+|ow9O7khhtB*|7&0@MZ z5=W__!&4kTCtbS(myFAO1LL1M+wdIMZOUY%#nK@UC7;$>7L9mXX6ua>8}9ezA8xecTR z2~3%XGU^b~jpdpN%noc1&2n+dpaRIa2^m;VoV_R65(oGWf0H7}CTr$YrgNhn;lD(q zExMSTM=kED(+>l6NupW46L&hwgpIlE!7%`St$0=lLSRgKQIO*~S&h$}GfO%Ui_m3v zu&3K1qvGUsUE=jMjyoPH9$CPPLTP#nwrU2^ujnm{Zf#wJ%2Wbx+6mAk%GjvTr^$5g z?GyLC(lw&~+P6$u-I^~C<2oF+zr2%a3&2K)m30^6{l}zUbIv$#y0cnm>)6yv)so*@ zulsGMXU?0;maKZV#gIjM)B2)lvQMXMsXNT8+Fe>RDUhy^)8C$o(KZi$nW~K(&A-5~ zotJ*>cs#?Ioo`PVk4Lg;$|(fbxGrL|Wb#VZzAJi3q*KA01bXAMvUZ7?*qV76$kaET zUDV@NxGAyT^r?FhoWap9S68J8YYdENo7S-F^27(vXO=vWnF;u6Z4dx1#>gsHp+Gyt?APJ2}!+I=N>L)zq+BnXM>JGDu31PY>(a z5nJvYYT>Z+8qGbV59p{vFSZ4Ju$WdYAO5+riUfE3)?Ah>a7@Xoojr7K?#sn1E8PU# zy-;EYY%1TT>yn#jW6^}d=dyvwq*4QXh4x!8HK53Ycr3UOY&3CGuTHk|nvHtfyG5cisJ1faW7vMPMU(Q;gz1T#$U;9R zMQZ+jOwZj^pe7Uu-~3#`3aCpyi6Hc&6icA7=A&{6Of<*m5`b6X#pGDy)oxl#rf`iH zc2Sb&qXn!fW#@XGSeGe8rG_EQ0d~IGE`HQX8NXLT1ckjrxx6KxK2kJ(a2XX(v;Qni zf)XdnfW2WahnA+f&m>RIt9`=u-gkEiX7ys1+u>{JUOQ{)#m4R7A~(Uz!0nE~wn19p z;bd<-l0f0@q_7aCh%uGCPWPhO1y?4eIWc)|QKuEgv4oAC-*en(&>*|js5(LO%59R- zcEQQUPR@F_rDExc6Ycej%iB7927;X&IGqG4IS1!=Oi{n6Kb5npHEPG^V#E-y}EZI)R^!MfMrPg}c7$n8aQf3}5HK*$p!uMx$08nJ= zxr?6NqUt!m6NgLrK^khOXGroSBfz+j_MU`B6fOpe9{2q4x}92>;ES7|wU_gnuN(-x z=thb|x({~Cs6OVP3H?Ir9}$03ail4 z{0!SvzrVp!1%+WTr|2FzWCbR~Z$NBBu8h7B?2PI<;m^>GzrRink5R*O;<=gN&%IyiSK&GWd- zr+v>IU4I48U`28oENwq$@*Tpw-%dxm*GClpx!j~$j5`!VjL)Onph>?zYNRPw$KGXo z%hXOOGv#>faWQ*SAhLfh#Qrg1E%O}I9ozd7eKZH7xb~ZveQ^roH6dtquu?l3BJ1O7 zTyHOss}59YG-s-dF&It6N!;yejvEbHu`#OubO-yJj_=tq8R;s1IaMhRNhs*%wY$A+TE#oDECnL_Ss}$|66(39 zdk3-Ci~d|{@?ZENRYpEA=V&4?nrI9%jzWbf)YNC`+F`LgGCUXIc~zA5gsdu*kVf@Z%(FCF%UGrwG{Hmjk6 zb`eGDmx**1RdCL)D!#iU*EQ$92A1yl<|~o?@j^r^DxhyE`zW%wSg_~z{A(CryzEh6?8J8xM>M-#H05b%wM_oV$H5Mh(Pp89O9ONIXH zSg%)|cr4Kh5oukqhWd{6(o}$TZs`RMC~n{D<*JnEt1LEb9Bdw1O!Ulm8?Cv_Bfjd- zrT!OR)47-r;(o4~_2jT%Zg|-TMWXso z59^f3_yT)Y=(wZIQxHwKTo$FzqMdW-yHYz2!#%+z{Cy%qgVFx+3 zXp?M;FQbgxt@Wy3&4-F#>JRlwN?C%=46_*FGdq+9zkqJfIg2Nkmb1?fuFtQ7Z}gkY z7U(u6Ae0yu6x*I!-92XpwKIAx`Q1MugeTCdZBgiX%lem~Nhpj2Y4!}@3^|!hvJLyo zZqe*SSBxXl$Y-cm5>9I67kIkZGaaB?>}JH03mXlU?H=kQ=yXvB=@6Efj~Ob@ahA%B z!bnEyS7Zs0^06Bzzp*A19rM>hbjnO&=^K|^Gt~-)Kyqr zCP>Ed#_3R`3t*FhB2y*qJ<(??LY#hY+sTscc*YM8;0%=qU6>4n2Ys*;%$GJi>8%J%3x~Ir78QIJP^sG? z=r)NqzE4|YZ9JlqjQGY#+?0U}Q>j(dDYAajg@{W3l$WA-P2}+4_(^Xe9BgFM2Pq8V z5C1U-N+W2p7XZ_O!_!IpdmIb|w?@-vzVLl1i6G~s9~qB@pBmGl6Z@WStf-S0FSVsd zRj1ux=V+u;qg`VbCkU+A3AmoaYUM+O|F!dqr zi90@GR6{d9MIZzXwBiKj4V0WP?oPc#k@1H=h7|q4AMb#c1Nq-etk81^{QtGY%9Q!a z*JA-!GBr8Aqo9R3n{A_rKwuub@nN`Uh%O&$b9rTAuE%S z6tfilgS`-k_tv@P9mOtCQ%J ztu$>uNIZ@S7_2 zWfQbJN^nU?UcWKEdv$t0V$ik&`hAqx6LF=JX|Qe-NTNMzO+pkAKw6M|EKE!%0)C%> z2-i4+l)p_EBT|E*DOIMBW%Eh!PaWQyy_cZH%qApIWRu3Isr=DkQw1XA zOKNan!=3IdkH}b-I^-t!&EylYQA2Jd^ohGEa)EzJ13<(O9uZjqE{_o)EG*2R$xyv{ z0U#Cm;d`pyeE7Q5zK~m9#)Yu~h(KmIG>dr4^g)Ir%`~OC;Y-;xVLc(qgj~^=ly|0| z4ba)p3&2N88Hw%{upY6!5|k2LAEiL5D|@c=K**uj3}adbs^&ao_RR z=l!P}da3!PBb{zbpYt=}<5k6k7;UfdE^5<1udm|9U>-Hoi{IwYr$;@{H{r}(o)%B0 zFEfQVBKHN`kK>PvSA>P9GlC<}cXKZOS*gvFV^5aZ(fFTrr!NcgZp(z6T0FK!^o00l z=W)mm1sVsjDMTsM0^CAEc*x@SJR z>R#<~)Txgb<@-<&4poIh@2jKxD6-g9)cEA@#B70+wK!6Tb7DboPGzjntv4c>`w z`**s|^PR3=Q{Ts!M zh5~3d%TkCyTxQI@#(Wy zPPpkOv{-V5tSKC(m(Hu_Zt|&|#`CG{MD54Et8P5j1D&Uy=iBHX0FveGj}@~1qC+Ui z0MZ3jVk%Mpo;|qIa}Lwdf|k=?-2>n2bB?(|siYzO6Zlz#sViL6d%~CAs||hPt7deV z6r#Dm;rhf2mw)iX!6Uzy%9ZwLvDm9<@lssRX+M5%=D6ZK-GBFsmD?odd@9SL1r-(! zo(m82pHau8P*xZ9U|=#QfrW!dzM`i9@{_|OWB!E;pxWFl(uo}iLdCnD{$u<=a8i_D@`|Nq)!d@*qdcUZeFbo5kd|C>i*&N#dWb(2^m&}?xwEVVAs8rgZPh5652ZutGf*p2~vf8&a1qK$b5MFgN zZ&m?S9-Hhf1>r}3n1CqwZ=q2ch3atd$Vs>iIAm@lK!y-BqIcDmUPAL-wOzBn`@|V} z6Ol0~lyzV?^Xe*92w>shGw7Ay1JTw&+F8!ClumblRcbG%dNbc@w^_RaIs8;sUWQk+ zJ1q=0`@V(tsUt!*AS&!8Wh5_S0t_r%G|D>^08%UwR%+H)TQtH3gUN*gd{zUjqxtGs zV{ycxKk+|JV-4IQG+qseQ`g)qS-hPrOEc*#yqCbGRg3)P+nr}F9@hfV+3${dP5yT45=UL&f0=K)6vOgc+XheyUN$@f23 z9;hk_hk=Dtz>^LRoe{pT<-aZw-|8@wkG0gQSlK!xUovgJyI7>}zY5*&uRim7uTyv) zN5kb00}H2vf(+1Ul&ghD#$=$6dkbg;^j3P?DR}!m7#z12mczgxW6DT~sgEGA30$NR z)E=>Mf)4FQYWu|hw+UqeKJDpQ4qb&>-U3ym4RA078GhYehZTFWeF1&4xSfZIyjV2jc$O4+j3yl z1&`+bTPRIDEMz7AKiyXQQxjJh-CZ{ARgi2bfl_f z3@TJTtI1NY}0MQE9*l1jVLofp}`tXb%Xg)G<7t@7HS7=livVw;wKuJfry| z2*vOT2ZEd!J+d8YR8C1r`C!x4*@6x^G=ef7 zWqjz{@x6)%iWeQ0%a&D>?KY2lKH1tZxQg|z*=`3hNeFUcq)ZMmDJUtK*_R!4Ws;-eaL=S;U3Oxt z6fEBh{=!5`XX0Dx74YZxJ=_2MUf2@Xgx1lHE&gv-*(LpV&urf9$as|PUp_yx4+!dt~V>pP8W7s-2?{FC0j3+;aAxCg_Uo$<6svTnXw`DJv0MbjUQP#MdjKm0${1EzP_q|qj-N2StbJxJ z4L#h0qPKK=Z-pK%K?R-Uqfe*7kaY~Z%Lx#aQH1Gb&VhHGpmH%((1{G?Q21>k4WGH^ zss&zFJ5q$7eSE6btV$J=w;3vHU7@Lg9wTp13-K%P!7#i8N#s2kXvyW1S%!S3K50_RSsu1 zidHsg1^n&M3Lj~R7tP4w^XUQ1WRJ5dDgkrcwN|3Gute`C5%tpJ=Nr<9d zq~Qw}hC_2OgfOh3cJ+~_{`qj<5Q1zDq360vs&VZ&ZVEVlH(=P@P~GthV;!v1@|*)^<@Z(^-2k{VPIw% z)R?D;&N3OA`yEkiqX$K+B2K14hGQfI+0Z>L1NO~E1zmPPfOR$~D(FypEu3jGD(Jdq zT%h4YX(hv&no9;2pjhQ>idL;zIF^r~m!v#6*PRM2>+qaStYJB$0El}>^5y?zfoKK8)~WV7hW*uxVyZvH^s)@K r&H=W5UA)YRAbkeOcFF$$l6mRelQ&~!u0azBf*=v0ABUU^-jw?%YTmCT literal 0 HcmV?d00001 diff --git a/Documentation/px4_hil/UserGuide.md b/Documentation/px4_hil/UserGuide.md new file mode 100644 index 0000000000..dd76d3e3a9 --- /dev/null +++ b/Documentation/px4_hil/UserGuide.md @@ -0,0 +1,98 @@ +[TOC] + +# Introduction + +The HIL architecture allows you to test the flight stack replacing the real physical vehicle and sensors with a simulator of vehicle dynamics and sensor outputs. The flight stack "is not aware" that it is not on a real vehicle. This is a powerful tool for develping and testing code rapidly in a benchtop environment. + +The flight stack can be run anywhere that supports a network connection to the simulator (with sufficient bandwidth and latency to transport the sensor and actuator messages). This can be on a standard linux workstation, an on-target linux image, or the on-target DSP image. These modes can be selected based on the goals of the testing. Workstation is useful for rapid testing in a tool-rich environment. DSP image testing is the closest to the final implementation, so is useful for testing actual HW operation, other than the physical sensing and actuation. + +## Px4 High-level HIL Architecture + +A diagram of the setup described is shown here. Note that UDP port numbers are only displayed on the socket server and are left blank on the socket client. + +(???NOTES: This diagram needs to be updated to use control inputs over UDP, either from QGC or from other) + +![SITL Diagram](./SITL_Diagram_QGC.png "SITL Diagram") + +## Requirements +The simulator that is currently supported is jMAVSim. The setup described here requires PX4 and jMAVSim installed and running. qGroundControl (QGC) is also required because it is the supported method of providing manual control commands. + +## Assumptions + +# Compiling Code + +## JMAVSim + +### Platform Requirements +Linux with java-1.7.x or greater + +### Build Instructions +In a clean directory +``` +> git clone https://github.com/PX4/jMAVSim.git +> cd jMAVSim +> git submodule init +> git submodule update +> ant +``` + +## qGroundControl + +### Platform Requirements +Windows 7 +Logitech Gamepad F310 joystick controller + +### Download/Install Instructions +Download QGC from http://qgroundcontrol.org/downloads and install using the windows executable. + + +## PX4 + +### Platfrom Requirements +Linux or Eagle with a working IP interface (?? does this need further instructions?) + +### Build Host Requirements +(???Notes: Windows?) + +### Download & Build Instructions + +### Installing binaries on the Qualcomm Target + +# Running PX4 in HIL Mode + +## Starting PX4 on Qualcomm Eagle + +``` +> adb shell +# bash +root@linaro-developer:/# cd ??? +root@linaro-developer:/# ./mainapp +App name: mainapp +Enter a command and its args: +uorb start +muorb start +mavlink start -u 14556 +simulator start -p +``` + +## Starting jMAVSim +In the directory where jMAVSim is installed +``` + java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp :14560 -n 100 +``` +replacing with the IP address of the machine running PX4 (Eagle). This can be found by running "ifconfig" on that machine. + +## Starting qGroundControl +Launch the qGroundControl application +1. Set up the communication to the flight stack. In the menu File:Settings:CommLinks, select Add. Enter a Link Name of your choice. Select Link Type: UDP. Set the listening port to an unused port (example: 14561). Select Add. Enter the IP address and port of the PX4 Mavlink app, which is :14556 with being the IP address of the Eagle board. Select OK. +1. Set up the joystick. Plug in the joystick to your Windows machine. In the menu File:Settings:CommLinks, check Enable Controllers. Select "Gamepad F310". Select "Manual". Set the axes/channel mapping to 0:Yaw, 1:Throttle, 2:unset, 3:Pitch, 4:Roll. Seletct "Inverted" for the throttle axis. Click "Calibrate range". Move the right joystick through its full range of motion. Move the left joystick full left then full right. Move the left joystick full forward (but not full backward). Click "end calibration." +1. Connect to the flight stack. Click Analyze. Click the "Connect" button in the upper right, and select the connection that you created in the first step. + +You should now be connected to the flight stack. You can see incoming Mavlink packets using the MAVLink Instpector (from Advanced:Tool Widgets) + + +## Controlling PX4 flight in HIL Mode +The joystick can now be used to fly the simulated vehicle. The jMAVSim world visualization gives a FPV view, and QGC can be used to display instruments such as artificial horizon and maps (if GPS simulation is enabled). + + +# Debugging/FAQ diff --git a/Documentation/px4_hil/docs/readme.txt b/Documentation/px4_hil/docs/readme.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Documentation/px4_hil/px4_hil.doxyfile b/Documentation/px4_hil/px4_hil.doxyfile new file mode 100644 index 0000000000..411b308be5 --- /dev/null +++ b/Documentation/px4_hil/px4_hil.doxyfile @@ -0,0 +1,2403 @@ +# Doxyfile 1.8.10 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all text +# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv +# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv +# for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "Px4 Hardware-In-the-Loop(HIL) User Guide for Qualcomm Eagle" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = ./docs + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding "class=itcl::class" +# will allow you to use the command class in the itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, Javascript, +# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran: +# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran: +# Fortran. In the later case the parser tries to guess whether the code is fixed +# or free formatted code, this is the default for Fortran type files), VHDL. For +# instance to make doxygen treat .inc files as Fortran files (default is PHP), +# and .f files as C (default is Fortran), use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See http://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# If one adds a struct or class to a group and this option is enabled, then also +# any nested class or struct is added to the same group. By default this option +# is disabled and one has to add nested compounds explicitly via \ingroup. +# The default value is: NO. + +GROUP_NESTED_COMPOUNDS = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = NO + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = NO + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# (class|struct|union) declarations. If set to NO, these declarations will be +# included in the documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file +# names in lower-case letters. If set to YES, upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. +# The default value is: system dependent. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong or incomplete +# parameter documentation, but not about the absence of documentation. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING +# Note: If this tag is empty the current directory is searched. + +INPUT = ./UserGuide.md + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: http://www.gnu.org/software/libiconv) for the list of +# possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# read by doxygen. +# +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, +# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, +# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, +# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, +# *.vhdl, *.ucf, *.qsf, *.as and *.js. + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = ./UserGuide.md + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = NO + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# function all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see http://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the config file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the +# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the +# cost of reduced performance. This can be particularly helpful with template +# rich C++ code for which doxygen's built-in parser lacks the necessary type +# information. +# Note: The availability of this option depends on whether or not doxygen was +# compiled with the --with-libclang option. +# The default value is: NO. + +CLANG_ASSISTED_PARSING = NO + +# If clang assisted parsing is enabled you can provide the compiler with command +# line options that you would normally use when invoking the compiler. Note that +# the include paths will already be set by doxygen for the files and directories +# specified with INPUT and INCLUDE_PATH. +# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. + +CLANG_OPTIONS = + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in +# which the alphabetical index list will be split. +# Minimum value: 1, maximum value: 20, default value: 5. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# http://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to YES can help to show when doxygen was last run and thus if the +# documentation is up to date. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = NO + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: http://developer.apple.com/tools/xcode/), introduced with +# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a +# Makefile in the HTML output directory. Running make will produce the docset in +# that directory and running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on +# Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the master .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- +# folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location of Qt's +# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the +# generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = YES + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# http://www.mathjax.org) which uses client side Javascript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from http://www.mathjax.org before deployment. +# The default value is: http://cdn.mathjax.org/mathjax/latest. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /