diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index d50eb1e500..43288537cb 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -63,9 +63,9 @@ MODULES += modules/gpio_led # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3_comp -#MODULES += modules/position_estimator_mc MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf +MODULES += examples/flow_position_estimator # # Vehicle Control @@ -75,11 +75,12 @@ MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control +MODULES += examples/flow_position_control +MODULES += examples/flow_speed_control # # Logging # -MODULES += modules/sdlog MODULES += modules/sdlog2 # diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index e50395e479..c39da98d70 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -37,7 +37,7 @@ * * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ - + #include #include @@ -77,13 +77,13 @@ #define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION /* I2C bus address */ -#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ +#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ /* Register address */ -#define READ_CMD 0x07 /* Read the data */ - +#define READ_CMD 0x07 /* Read the data */ + /** - * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. + * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. */ #define MIN_ACCURATE_DIFF_PRES_PA 12 @@ -105,38 +105,38 @@ class ETSAirspeed : public device::I2C public: ETSAirspeed(int bus, int address = I2C_ADDRESS); virtual ~ETSAirspeed(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + /** * Diagnostics - print some basic information about the driver. */ - void print_info(); - + void print_info(); + protected: - virtual int probe(); + virtual int probe(); private: - work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - differential_pressure_s *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; - int _diff_pres_offset; - - orb_advert_t _airspeed_pub; + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + differential_pressure_s *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _diff_pres_offset; + + orb_advert_t _airspeed_pub; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - /** * Test whether the device supported by the driver is present at a * specific address. @@ -144,28 +144,28 @@ private: * @param address The I2C bus address to probe. * @return True if the device is present. */ - int probe_address(uint8_t address); - + int probe_address(uint8_t address); + /** * Initialise the automatic measurement state machine and start it. * * @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(); - + void start(); + /** * Stop the automatic measurement state machine. */ - void stop(); - + void stop(); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); - int measure(); - int collect(); + void cycle(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a @@ -173,9 +173,9 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); - - + static void cycle_trampoline(void *arg); + + }; /* helper macro for handling report buffer indices */ @@ -203,7 +203,7 @@ ETSAirspeed::ETSAirspeed(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -230,6 +230,7 @@ ETSAirspeed::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct differential_pressure_s[_num_reports]; + for (unsigned i = 0; i < _num_reports; i++) _reports[i].max_differential_pressure_pa = 0; @@ -351,11 +352,11 @@ ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGQUEUEDEPTH: return _num_reports - 1; - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -432,14 +433,14 @@ ETSAirspeed::measure() uint8_t cmd = READ_CMD; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); return ret; } + ret = OK; - + return ret; } @@ -447,30 +448,31 @@ int ETSAirspeed::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[2] = {0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 2); - + if (ret < 0) { log("error reading from sensor: %d", ret); return ret; } - + uint16_t diff_pres_pa = val[1] << 8 | val[0]; param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); - - if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { + + if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; + } else { - diff_pres_pa -= _diff_pres_offset; + diff_pres_pa -= _diff_pres_offset; } - // XXX we may want to smooth out the readings to remove noise. + // XXX we may want to smooth out the readings to remove noise. _reports[_next_report].timestamp = hrt_absolute_time(); _reports[_next_report].differential_pressure_pa = diff_pres_pa; @@ -498,7 +500,7 @@ ETSAirspeed::collect() ret = OK; perf_end(_sample_perf); - + return ret; } @@ -511,17 +513,19 @@ ETSAirspeed::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, true, true, - SUBSYSTEM_TYPE_DIFFPRESSURE}; + SUBSYSTEM_TYPE_DIFFPRESSURE + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -653,8 +657,7 @@ start(int i2c_bus) fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -668,15 +671,14 @@ fail: void stop() { - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + + } else { errx(1, "driver not running"); } + exit(0); } @@ -773,10 +775,10 @@ info() } // namespace -static void -ets_airspeed_usage() +static void +ets_airspeed_usage() { - fprintf(stderr, "usage: ets_airspeed [options] command\n"); + fprintf(stderr, "usage: ets_airspeed command [options]\n"); fprintf(stderr, "options:\n"); fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT); fprintf(stderr, "command:\n"); @@ -789,6 +791,7 @@ ets_airspeed_main(int argc, char *argv[]) int i2c_bus = PX4_I2C_BUS_DEFAULT; int i; + for (i = 1; i < argc; i++) { if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { if (argc > i + 1) { @@ -802,12 +805,12 @@ ets_airspeed_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) ets_airspeed::start(i2c_bus); - - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - ets_airspeed::stop(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + ets_airspeed::stop(); /* * Test the driver/device. diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index 369070f8c4..d2634ef41a 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -56,6 +57,7 @@ static int battery_sub = -1; static int gps_sub = -1; static int home_sub = -1; static int sensor_sub = -1; +static int airspeed_sub = -1; static bool home_position_set = false; static double home_lat = 0.0d; @@ -68,6 +70,7 @@ messages_init(void) gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); home_sub = orb_subscribe(ORB_ID(home_position)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + airspeed_sub = orb_subscribe(ORB_ID(airspeed)); } void @@ -100,6 +103,16 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; + /* get a local copy of the airspeed data */ + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); + + uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s); + msg.speed_L = (uint8_t)speed & 0xff; + msg.speed_H = (uint8_t)(speed >> 8) & 0xff; + + msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); } diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c new file mode 100644 index 0000000000..c177c8fd20 --- /dev/null +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -0,0 +1,589 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 flow_position_control.c + * + * Optical flow position controller + */ + +#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 "flow_position_control_params.h" + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +__EXPORT int flow_position_control_main(int argc, char *argv[]); + +/** + * Mainloop of position controller. + */ +static int flow_position_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_spawn(). + */ +int flow_position_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow position control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("flow_position_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 4096, + flow_position_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) + { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) + { + if (thread_running) + printf("\tflow position control app is running\n"); + else + printf("\tflow position control app not started\n"); + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int +flow_position_control_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + printf("[flow position control] starting\n"); + + uint32_t counter = 0; + const float time_scale = powf(10.0f,-6.0f); + + /* structures */ + struct vehicle_status_s vstatus; + struct vehicle_attitude_s att; + struct manual_control_setpoint_s manual; + struct filtered_bottom_flow_s filtered_flow; + struct vehicle_local_position_s local_pos; + + struct vehicle_bodyframe_speed_setpoint_s speed_sp; + + /* subscribe to attitude, motor setpoints and system state */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); + int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + + orb_advert_t speed_sp_pub; + bool speed_setpoint_adverted = false; + + /* parameters init*/ + struct flow_position_control_params params; + struct flow_position_control_param_handles param_handles; + parameters_init(¶m_handles); + parameters_update(¶m_handles, ¶ms); + + /* init flow sum setpoint */ + float flow_sp_sumx = 0.0f; + float flow_sp_sumy = 0.0f; + + /* init yaw setpoint */ + float yaw_sp = 0.0f; + + /* init height setpoint */ + float height_sp = params.height_min; + + /* height controller states */ + bool start_phase = true; + bool landing_initialized = false; + float landing_thrust_start = 0.0f; + + /* states */ + float integrated_h_error = 0.0f; + float last_local_pos_z = 0.0f; + bool update_flow_sp_sumx = false; + bool update_flow_sp_sumy = false; + uint64_t last_time = 0.0f; + float dt = 0.0f; // s + + + /* register the perf counter */ + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err"); + + static bool sensors_ready = false; + + while (!thread_should_exit) + { + /* wait for first attitude msg to be sure all data are available */ + if (sensors_ready) + { + /* polling */ + struct pollfd fds[2] = { + { .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator + { .fd = parameter_update_sub, .events = POLLIN } + + }; + + /* wait for a position update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ +// printf("[flow position control] no filtered flow updates\n"); + } + else + { + /* parameter update available? */ + if (fds[1].revents & POLLIN) + { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + parameters_update(¶m_handles, ¶ms); + printf("[flow position control] parameters updated.\n"); + } + + /* only run controller if position/speed changed */ + if (fds[0].revents & POLLIN) + { + perf_begin(mc_loop_perf); + + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + /* get a local copy of filtered bottom flow */ + orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); + /* get a local copy of local position */ + orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); + + if (vstatus.state_machine == SYSTEM_STATE_AUTO) + { + float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 + float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 + float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1 + + /* calc dt */ + if(last_time == 0) + { + last_time = hrt_absolute_time(); + continue; + } + dt = ((float) (hrt_absolute_time() - last_time)) * time_scale; + last_time = hrt_absolute_time(); + + /* update flow sum setpoint */ + if (update_flow_sp_sumx) + { + flow_sp_sumx = filtered_flow.sumx; + update_flow_sp_sumx = false; + } + if (update_flow_sp_sumy) + { + flow_sp_sumy = filtered_flow.sumy; + update_flow_sp_sumy = false; + } + + /* calc new bodyframe speed setpoints */ + float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d; + float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d; + float speed_limit_height_factor = height_sp; // the settings are for 1 meter + + /* overwrite with rc input if there is any */ + if(isfinite(manual_pitch) && isfinite(manual_roll)) + { + if(fabsf(manual_pitch) > params.manual_threshold) + { + speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor; + update_flow_sp_sumx = true; + } + + if(fabsf(manual_roll) > params.manual_threshold) + { + speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor; + update_flow_sp_sumy = true; + } + } + + /* limit speed setpoints */ + if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) && + (speed_body_x >= -params.limit_speed_x * speed_limit_height_factor)) + { + speed_sp.vx = speed_body_x; + } + else + { + if(speed_body_x > params.limit_speed_x * speed_limit_height_factor) + speed_sp.vx = params.limit_speed_x * speed_limit_height_factor; + if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor) + speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor; + } + + if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) && + (speed_body_y >= -params.limit_speed_y * speed_limit_height_factor)) + { + speed_sp.vy = speed_body_y; + } + else + { + if(speed_body_y > params.limit_speed_y * speed_limit_height_factor) + speed_sp.vy = params.limit_speed_y * speed_limit_height_factor; + if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor) + speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor; + } + + /* manual yaw change */ + if(isfinite(manual_yaw) && isfinite(manual.throttle)) + { + if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f) + { + yaw_sp += manual_yaw * params.limit_yaw_step; + + /* modulo for rotation -pi +pi */ + if(yaw_sp < -M_PI_F) + yaw_sp = yaw_sp + M_TWOPI_F; + else if(yaw_sp > M_PI_F) + yaw_sp = yaw_sp - M_TWOPI_F; + } + } + + /* forward yaw setpoint */ + speed_sp.yaw_sp = yaw_sp; + + + /* manual height control + * 0-20%: thrust linear down + * 20%-40%: down + * 40%-60%: stabilize altitude + * 60-100%: up + */ + float thrust_control = 0.0f; + + if (isfinite(manual.throttle)) + { + if (start_phase) + { + /* control start thrust with stick input */ + if (manual.throttle < 0.4f) + { + /* first 40% for up to feedforward */ + thrust_control = manual.throttle / 0.4f * params.thrust_feedforward; + } + else + { + /* second 60% for up to feedforward + 10% */ + thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward; + } + + /* exit start phase if setpoint is reached */ + if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower) + { + start_phase = false; + /* switch to stabilize */ + thrust_control = params.thrust_feedforward; + } + } + else + { + if (manual.throttle < 0.2f) + { + /* landing initialization */ + if (!landing_initialized) + { + /* consider last thrust control to avoid steps */ + landing_thrust_start = speed_sp.thrust_sp; + landing_initialized = true; + } + + /* set current height as setpoint to avoid steps */ + if (-local_pos.z > params.height_min) + height_sp = -local_pos.z; + else + height_sp = params.height_min; + + /* lower 20% stick range controls thrust down */ + thrust_control = manual.throttle / 0.2f * landing_thrust_start; + + /* assume ground position here */ + if (thrust_control < 0.1f) + { + /* reset integral if on ground */ + integrated_h_error = 0.0f; + /* switch to start phase */ + start_phase = true; + /* reset height setpoint */ + height_sp = params.height_min; + } + } + else + { + /* stabilized mode */ + landing_initialized = false; + + /* calc new thrust with PID */ + float height_error = (local_pos.z - (-height_sp)); + + /* update height setpoint if needed*/ + if (manual.throttle < 0.4f) + { + /* down */ + if (height_sp > params.height_min + params.height_rate && + fabsf(height_error) < params.limit_height_error) + height_sp -= params.height_rate * dt; + } + + if (manual.throttle > 0.6f) + { + /* up */ + if (height_sp < params.height_max && + fabsf(height_error) < params.limit_height_error) + height_sp += params.height_rate * dt; + } + + /* instead of speed limitation, limit height error (downwards) */ + if(height_error > params.limit_height_error) + height_error = params.limit_height_error; + else if(height_error < -params.limit_height_error) + height_error = -params.limit_height_error; + + integrated_h_error = integrated_h_error + height_error; + float integrated_thrust_addition = integrated_h_error * params.height_i; + + if(integrated_thrust_addition > params.limit_thrust_int) + integrated_thrust_addition = params.limit_thrust_int; + if(integrated_thrust_addition < -params.limit_thrust_int) + integrated_thrust_addition = -params.limit_thrust_int; + + float height_speed = last_local_pos_z - local_pos.z; + float thrust_diff = height_error * params.height_p - height_speed * params.height_d; + + thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition; + + /* add attitude component + * F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM + */ +// // TODO problem with attitude +// if (att.R_valid && att.R[2][2] > 0) +// thrust_control = thrust_control / att.R[2][2]; + + /* set thrust lower limit */ + if(thrust_control < params.limit_thrust_lower) + thrust_control = params.limit_thrust_lower; + } + } + + /* set thrust upper limit */ + if(thrust_control > params.limit_thrust_upper) + thrust_control = params.limit_thrust_upper; + } + /* store actual height for speed estimation */ + last_local_pos_z = local_pos.z; + + speed_sp.thrust_sp = thrust_control; + speed_sp.timestamp = hrt_absolute_time(); + + /* publish new speed setpoint */ + if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp)) + { + + if(speed_setpoint_adverted) + { + orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp); + } + else + { + speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp); + speed_setpoint_adverted = true; + } + } + else + { + warnx("NaN in flow position controller!"); + } + } + else + { + /* in manual or stabilized state just reset speed and flow sum setpoint */ + speed_sp.vx = 0.0f; + speed_sp.vy = 0.0f; + flow_sp_sumx = filtered_flow.sumx; + flow_sp_sumy = filtered_flow.sumy; + if(isfinite(att.yaw)) + { + yaw_sp = att.yaw; + speed_sp.yaw_sp = att.yaw; + } + if(isfinite(manual.throttle)) + speed_sp.thrust_sp = manual.throttle; + } + + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + perf_end(mc_loop_perf); + } + } + + counter++; + } + else + { + /* sensors not ready waiting for first attitude msg */ + + /* polling */ + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + /* wait for a flow msg, check for exit condition every 5 s */ + int ret = poll(fds, 1, 5000); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ + printf("[flow position control] no attitude received.\n"); + } + else + { + if (fds[0].revents & POLLIN) + { + sensors_ready = true; + printf("[flow position control] initialized.\n"); + } + } + } + } + + printf("[flow position control] ending now...\n"); + + thread_running = false; + + close(parameter_update_sub); + close(vehicle_attitude_sub); + close(vehicle_local_position_sub); + close(vehicle_status_sub); + close(manual_control_setpoint_sub); + close(speed_sp_pub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + return 0; +} + diff --git a/src/examples/flow_position_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c new file mode 100644 index 0000000000..4f3598a578 --- /dev/null +++ b/src/examples/flow_position_control/flow_position_control_params.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 flow_position_control_params.c + */ + +#include "flow_position_control_params.h" + +/* controller parameters */ +PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f); +PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f); +PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f); +PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f); +PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f); +PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f); +PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f); +PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f); +PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight +PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f); +PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f); +PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f); +PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f); +PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f); +PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f); +PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f); +PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f); + + +int parameters_init(struct flow_position_control_param_handles *h) +{ + /* PID parameters */ + h->pos_p = param_find("FPC_POS_P"); + h->pos_d = param_find("FPC_POS_D"); + h->height_p = param_find("FPC_H_P"); + h->height_i = param_find("FPC_H_I"); + h->height_d = param_find("FPC_H_D"); + h->height_rate = param_find("FPC_H_RATE"); + h->height_min = param_find("FPC_H_MIN"); + h->height_max = param_find("FPC_H_MAX"); + h->thrust_feedforward = param_find("FPC_T_FFWD"); + h->limit_speed_x = param_find("FPC_L_S_X"); + h->limit_speed_y = param_find("FPC_L_S_Y"); + h->limit_height_error = param_find("FPC_L_H_ERR"); + h->limit_thrust_int = param_find("FPC_L_TH_I"); + h->limit_thrust_upper = param_find("FPC_L_TH_U"); + h->limit_thrust_lower = param_find("FPC_L_TH_L"); + h->limit_yaw_step = param_find("FPC_L_YAW_STEP"); + h->manual_threshold = param_find("FPC_MAN_THR"); + h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); + h->rc_scale_roll = param_find("RC_SCALE_ROLL"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); + + return OK; +} + +int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p) +{ + param_get(h->pos_p, &(p->pos_p)); + param_get(h->pos_d, &(p->pos_d)); + param_get(h->height_p, &(p->height_p)); + param_get(h->height_i, &(p->height_i)); + param_get(h->height_d, &(p->height_d)); + param_get(h->height_rate, &(p->height_rate)); + param_get(h->height_min, &(p->height_min)); + param_get(h->height_max, &(p->height_max)); + param_get(h->thrust_feedforward, &(p->thrust_feedforward)); + param_get(h->limit_speed_x, &(p->limit_speed_x)); + param_get(h->limit_speed_y, &(p->limit_speed_y)); + param_get(h->limit_height_error, &(p->limit_height_error)); + param_get(h->limit_thrust_int, &(p->limit_thrust_int)); + param_get(h->limit_thrust_upper, &(p->limit_thrust_upper)); + param_get(h->limit_thrust_lower, &(p->limit_thrust_lower)); + param_get(h->limit_yaw_step, &(p->limit_yaw_step)); + param_get(h->manual_threshold, &(p->manual_threshold)); + param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); + param_get(h->rc_scale_roll, &(p->rc_scale_roll)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); + + return OK; +} diff --git a/src/examples/flow_position_control/flow_position_control_params.h b/src/examples/flow_position_control/flow_position_control_params.h new file mode 100644 index 0000000000..d0c8fc722f --- /dev/null +++ b/src/examples/flow_position_control/flow_position_control_params.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 flow_position_control_params.h + * + * Parameters for position controller + */ + +#include + +struct flow_position_control_params { + float pos_p; + float pos_d; + float height_p; + float height_i; + float height_d; + float height_rate; + float height_min; + float height_max; + float thrust_feedforward; + float limit_speed_x; + float limit_speed_y; + float limit_height_error; + float limit_thrust_int; + float limit_thrust_upper; + float limit_thrust_lower; + float limit_yaw_step; + float manual_threshold; + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; +}; + +struct flow_position_control_param_handles { + param_t pos_p; + param_t pos_d; + param_t height_p; + param_t height_i; + param_t height_d; + param_t height_rate; + param_t height_min; + param_t height_max; + param_t thrust_feedforward; + param_t limit_speed_x; + param_t limit_speed_y; + param_t limit_height_error; + param_t limit_thrust_int; + param_t limit_thrust_upper; + param_t limit_thrust_lower; + param_t limit_yaw_step; + param_t manual_threshold; + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_position_control_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p); diff --git a/src/examples/flow_position_control/module.mk b/src/examples/flow_position_control/module.mk new file mode 100644 index 0000000000..b10dc490ac --- /dev/null +++ b/src/examples/flow_position_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build multirotor position control +# + +MODULE_COMMAND = flow_position_control + +SRCS = flow_position_control_main.c \ + flow_position_control_params.c diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c new file mode 100644 index 0000000000..c0b16d2e78 --- /dev/null +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -0,0 +1,458 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 flow_position_estimator_main.c + * + * Optical flow position estimator + */ + +#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 "flow_position_estimator_params.h" + +__EXPORT int flow_position_estimator_main(int argc, char *argv[]); +static bool thread_should_exit = false; /**< Daemon exit flag */ +static bool thread_running = false; /**< Daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +int flow_position_estimator_thread_main(int argc, char *argv[]); +static void usage(const char *reason); + +static void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int flow_position_estimator_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow position estimator already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn("flow_position_estimator", + SCHED_RR, + SCHED_PRIORITY_MAX - 5, + 4096, + flow_position_estimator_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) + { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) + { + if (thread_running) + printf("\tflow position estimator is running\n"); + else + printf("\tflow position estimator not started\n"); + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int flow_position_estimator_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + printf("[flow position estimator] starting\n"); + + /* rotation matrix for transformation of optical flow speed vectors */ + static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 }, + { -1, 0, 0 }, + { 0, 0, 1 }}; // 90deg rotated + const float time_scale = powf(10.0f,-6.0f); + static float speed[3] = {0.0f, 0.0f, 0.0f}; + static float flow_speed[3] = {0.0f, 0.0f, 0.0f}; + static float global_speed[3] = {0.0f, 0.0f, 0.0f}; + static uint32_t counter = 0; + static uint64_t time_last_flow = 0; // in ms + static float dt = 0.0f; // seconds + static float sonar_last = 0.0f; + static bool sonar_valid = false; + static float sonar_lp = 0.0f; + + /* subscribe to vehicle status, attitude, sensors and flow*/ + struct vehicle_status_s vstatus; + memset(&vstatus, 0, sizeof(vstatus)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); + + /* subscribe to parameter changes */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to vehicle status */ + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* subscribe to attitude */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + /* subscribe to attitude setpoint */ + int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + + /* subscribe to optical flow*/ + int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); + + /* init local position and filtered flow struct */ + struct vehicle_local_position_s local_pos = { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f, + .vx = 0.0f, + .vy = 0.0f, + .vz = 0.0f + }; + struct filtered_bottom_flow_s filtered_flow = { + .sumx = 0.0f, + .sumy = 0.0f, + .vx = 0.0f, + .vy = 0.0f + }; + + /* advert pub messages */ + orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); + orb_advert_t filtered_flow_pub = orb_advertise(ORB_ID(filtered_bottom_flow), &filtered_flow); + + /* vehicle flying status parameters */ + bool vehicle_liftoff = false; + bool sensors_ready = false; + + /* parameters init*/ + struct flow_position_estimator_params params; + struct flow_position_estimator_param_handles param_handles; + parameters_init(¶m_handles); + parameters_update(¶m_handles, ¶ms); + + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_estimator_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err"); + + while (!thread_should_exit) + { + if (sensors_ready) + { + /*This runs at the rate of the sensors */ + struct pollfd fds[2] = { + { .fd = optical_flow_sub, .events = POLLIN }, + { .fd = parameter_update_sub, .events = POLLIN } + }; + + /* wait for a sensor update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + + } + else if (ret == 0) + { + /* no return value, ignore */ +// printf("[flow position estimator] no bottom flow.\n"); + } + else + { + + /* parameter update available? */ + if (fds[1].revents & POLLIN) + { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + parameters_update(¶m_handles, ¶ms); + printf("[flow position estimator] parameters updated.\n"); + } + + /* only if flow data changed */ + if (fds[0].revents & POLLIN) + { + perf_begin(mc_loop_perf); + + orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + /* got flow, updating attitude and status as well */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + + /* vehicle state estimation */ + float sonar_new = flow.ground_distance_m; + + /* set liftoff boolean + * -> at bottom sonar sometimes does not work correctly, and has to be calibrated (distance higher than 0.3m) + * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time) + * -> minimum sonar value 0.3m + */ + if (!vehicle_liftoff) + { + if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + vehicle_liftoff = true; + } + else + { + if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + vehicle_liftoff = false; + } + + /* calc dt between flow timestamps */ + /* ignore first flow msg */ + if(time_last_flow == 0) + { + time_last_flow = flow.timestamp; + continue; + } + dt = (float)(flow.timestamp - time_last_flow) * time_scale ; + time_last_flow = flow.timestamp; + + /* only make position update if vehicle is lift off or DEBUG is activated*/ + if (vehicle_liftoff || params.debug) + { + /* copy flow */ + flow_speed[0] = flow.flow_comp_x_m; + flow_speed[1] = flow.flow_comp_y_m; + flow_speed[2] = 0.0f; + + /* convert to bodyframe velocity */ + for(uint8_t i = 0; i < 3; i++) + { + float sum = 0.0f; + for(uint8_t j = 0; j < 3; j++) + sum = sum + flow_speed[j] * rotM_flow_sensor[j][i]; + + speed[i] = sum; + } + + /* update filtered flow */ + filtered_flow.sumx = filtered_flow.sumx + speed[0] * dt; + filtered_flow.sumy = filtered_flow.sumy + speed[1] * dt; + filtered_flow.vx = speed[0]; + filtered_flow.vy = speed[1]; + + // TODO add yaw rotation correction (with distance to vehicle zero) + + /* convert to globalframe velocity + * -> local position is currently not used for position control + */ + for(uint8_t i = 0; i < 3; i++) + { + float sum = 0.0f; + for(uint8_t j = 0; j < 3; j++) + sum = sum + speed[j] * att.R[i][j]; + + global_speed[i] = sum; + } + + local_pos.x = local_pos.x + global_speed[0] * dt; + local_pos.y = local_pos.y + global_speed[1] * dt; + local_pos.vx = global_speed[0]; + local_pos.vy = global_speed[1]; + } + else + { + /* set speed to zero and let position as it is */ + filtered_flow.vx = 0; + filtered_flow.vy = 0; + local_pos.vx = 0; + local_pos.vy = 0; + } + + /* filtering ground distance */ + if (!vehicle_liftoff || !vstatus.flag_system_armed) + { + /* not possible to fly */ + sonar_valid = false; + local_pos.z = 0.0f; + } + else + { + sonar_valid = true; + } + + if (sonar_valid || params.debug) + { + /* simple lowpass sonar filtering */ + /* if new value or with sonar update frequency */ + if (sonar_new != sonar_last || counter % 10 == 0) + { + sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp; + sonar_last = sonar_new; + } + + float height_diff = sonar_new - sonar_lp; + + /* if over 1/2m spike follow lowpass */ + if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) + { + local_pos.z = -sonar_lp; + } + else + { + local_pos.z = -sonar_new; + } + } + + filtered_flow.timestamp = hrt_absolute_time(); + local_pos.timestamp = hrt_absolute_time(); + + /* publish local position */ + if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) + && isfinite(local_pos.vx) && isfinite(local_pos.vy)) + { + orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); + } + + /* publish filtered flow */ + if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy)) + { + orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow); + } + + /* measure in what intervals the position estimator runs */ + perf_count(mc_interval_perf); + perf_end(mc_loop_perf); + + } + } + + } + else + { + /* sensors not ready waiting for first attitude msg */ + + /* polling */ + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + /* wait for a attitude message, check for exit condition every 5 s */ + int ret = poll(fds, 1, 5000); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ + printf("[flow position estimator] no attitude received.\n"); + } + else + { + if (fds[0].revents & POLLIN){ + sensors_ready = true; + printf("[flow position estimator] initialized.\n"); + } + } + } + + counter++; + } + + printf("[flow position estimator] exiting.\n"); + thread_running = false; + + close(vehicle_attitude_setpoint_sub); + close(vehicle_attitude_sub); + close(vehicle_status_sub); + close(parameter_update_sub); + close(optical_flow_sub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + return 0; +} + + diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c new file mode 100644 index 0000000000..ec3c3352d0 --- /dev/null +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 flow_position_estimator_params.c + * + * Parameters for position estimator + */ + +#include "flow_position_estimator_params.h" + +/* Extended Kalman Filter covariances */ + +/* controller parameters */ +PARAM_DEFINE_FLOAT(FPE_LO_THRUST, 0.4f); +PARAM_DEFINE_FLOAT(FPE_SONAR_LP_U, 0.5f); +PARAM_DEFINE_FLOAT(FPE_SONAR_LP_L, 0.2f); +PARAM_DEFINE_INT32(FPE_DEBUG, 0); + + +int parameters_init(struct flow_position_estimator_param_handles *h) +{ + /* PID parameters */ + h->minimum_liftoff_thrust = param_find("FPE_LO_THRUST"); + h->sonar_upper_lp_threshold = param_find("FPE_SONAR_LP_U"); + h->sonar_lower_lp_threshold = param_find("FPE_SONAR_LP_L"); + h->debug = param_find("FPE_DEBUG"); + + return OK; +} + +int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p) +{ + param_get(h->minimum_liftoff_thrust, &(p->minimum_liftoff_thrust)); + param_get(h->sonar_upper_lp_threshold, &(p->sonar_upper_lp_threshold)); + param_get(h->sonar_lower_lp_threshold, &(p->sonar_lower_lp_threshold)); + param_get(h->debug, &(p->debug)); + + return OK; +} diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h new file mode 100644 index 0000000000..f9a9bb303b --- /dev/null +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 flow_position_estimator_params.h + * + * Parameters for position estimator + */ + +#include + +struct flow_position_estimator_params { + float minimum_liftoff_thrust; + float sonar_upper_lp_threshold; + float sonar_lower_lp_threshold; + int debug; +}; + +struct flow_position_estimator_param_handles { + param_t minimum_liftoff_thrust; + param_t sonar_upper_lp_threshold; + param_t sonar_lower_lp_threshold; + param_t debug; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_position_estimator_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p); diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk new file mode 100644 index 0000000000..88c9ceb937 --- /dev/null +++ b/src/examples/flow_position_estimator/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build position estimator +# + +MODULE_COMMAND = flow_position_estimator + +SRCS = flow_position_estimator_main.c \ + flow_position_estimator_params.c diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c new file mode 100644 index 0000000000..9648728c81 --- /dev/null +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -0,0 +1,361 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 flow_speed_control.c + * + * Optical flow speed controller + */ + +#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 "flow_speed_control_params.h" + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +__EXPORT int flow_speed_control_main(int argc, char *argv[]); + +/** + * Mainloop of position controller. + */ +static int flow_speed_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_spawn(). + */ +int flow_speed_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow speed control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("flow_speed_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 4096, + flow_speed_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) + { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) + { + if (thread_running) + printf("\tflow speed control app is running\n"); + else + printf("\tflow speed control app not started\n"); + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int +flow_speed_control_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + printf("[flow speed control] starting\n"); + + uint32_t counter = 0; + + /* structures */ + struct vehicle_status_s vstatus; + struct filtered_bottom_flow_s filtered_flow; + struct vehicle_bodyframe_speed_setpoint_s speed_sp; + + struct vehicle_attitude_setpoint_s att_sp; + + /* subscribe to attitude, motor setpoints and system state */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); + int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); + + orb_advert_t att_sp_pub; + bool attitude_setpoint_adverted = false; + + /* parameters init*/ + struct flow_speed_control_params params; + struct flow_speed_control_param_handles param_handles; + parameters_init(¶m_handles); + parameters_update(¶m_handles, ¶ms); + + /* register the perf counter */ + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err"); + + static bool sensors_ready = false; + + while (!thread_should_exit) + { + /* wait for first attitude msg to be sure all data are available */ + if (sensors_ready) + { + /* polling */ + struct pollfd fds[2] = { + { .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller + { .fd = parameter_update_sub, .events = POLLIN } + }; + + /* wait for a position update, check for exit condition every 5000 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ +// printf("[flow speed control] no bodyframe speed setpoints updates\n"); + } + else + { + /* parameter update available? */ + if (fds[1].revents & POLLIN) + { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + parameters_update(¶m_handles, ¶ms); + printf("[flow speed control] parameters updated.\n"); + } + + /* only run controller if position/speed changed */ + if (fds[0].revents & POLLIN) + { + perf_begin(mc_loop_perf); + + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of filtered bottom flow */ + orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); + /* get a local copy of bodyframe speed setpoint */ + orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); + + if (vstatus.state_machine == SYSTEM_STATE_AUTO) + { + /* calc new roll/pitch */ + float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; + float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p; + + /* limit roll and pitch corrections */ + if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch)) + { + att_sp.pitch_body = pitch_body; + } + else + { + if(pitch_body > params.limit_pitch) + att_sp.pitch_body = params.limit_pitch; + if(pitch_body < -params.limit_pitch) + att_sp.pitch_body = -params.limit_pitch; + } + + if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll)) + { + att_sp.roll_body = roll_body; + } + else + { + if(roll_body > params.limit_roll) + att_sp.roll_body = params.limit_roll; + if(roll_body < -params.limit_roll) + att_sp.roll_body = -params.limit_roll; + } + + /* set yaw setpoint forward*/ + att_sp.yaw_body = speed_sp.yaw_sp; + + /* add trim from parameters */ + att_sp.roll_body = att_sp.roll_body + params.trim_roll; + att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch; + + att_sp.thrust = speed_sp.thrust_sp; + att_sp.timestamp = hrt_absolute_time(); + + /* publish new attitude setpoint */ + if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust)) + { + if (attitude_setpoint_adverted) + { + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + else + { + att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + attitude_setpoint_adverted = true; + } + } + else + { + warnx("NaN in flow speed controller!"); + } + } + else + { + /* reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.thrust = 0.0f; + att_sp.yaw_body = 0.0f; + } + + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + perf_end(mc_loop_perf); + } + } + + counter++; + } + else + { + /* sensors not ready waiting for first attitude msg */ + + /* polling */ + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + /* wait for a flow msg, check for exit condition every 5 s */ + int ret = poll(fds, 1, 5000); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ + printf("[flow speed control] no attitude received.\n"); + } + else + { + if (fds[0].revents & POLLIN) + { + sensors_ready = true; + printf("[flow speed control] initialized.\n"); + } + } + } + } + + printf("[flow speed control] ending now...\n"); + + thread_running = false; + + close(parameter_update_sub); + close(vehicle_attitude_sub); + close(vehicle_bodyframe_speed_setpoint_sub); + close(filtered_bottom_flow_sub); + close(vehicle_status_sub); + close(att_sp_pub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + return 0; +} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.c b/src/examples/flow_speed_control/flow_speed_control_params.c new file mode 100644 index 0000000000..8dfe541736 --- /dev/null +++ b/src/examples/flow_speed_control/flow_speed_control_params.c @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 flow_speed_control_params.c + * + */ + +#include "flow_speed_control_params.h" + +/* controller parameters */ +PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f); +PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f); +PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f); + +int parameters_init(struct flow_speed_control_param_handles *h) +{ + /* PID parameters */ + h->speed_p = param_find("FSC_S_P"); + h->limit_pitch = param_find("FSC_L_PITCH"); + h->limit_roll = param_find("FSC_L_ROLL"); + h->trim_roll = param_find("TRIM_ROLL"); + h->trim_pitch = param_find("TRIM_PITCH"); + + + return OK; +} + +int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p) +{ + param_get(h->speed_p, &(p->speed_p)); + param_get(h->limit_pitch, &(p->limit_pitch)); + param_get(h->limit_roll, &(p->limit_roll)); + param_get(h->trim_roll, &(p->trim_roll)); + param_get(h->trim_pitch, &(p->trim_pitch)); + + return OK; +} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.h b/src/examples/flow_speed_control/flow_speed_control_params.h new file mode 100644 index 0000000000..eec27a2bf7 --- /dev/null +++ b/src/examples/flow_speed_control/flow_speed_control_params.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 flow_speed_control_params.h + * + * Parameters for speed controller + */ + +#include + +struct flow_speed_control_params { + float speed_p; + float limit_pitch; + float limit_roll; + float trim_roll; + float trim_pitch; +}; + +struct flow_speed_control_param_handles { + param_t speed_p; + param_t limit_pitch; + param_t limit_roll; + param_t trim_roll; + param_t trim_pitch; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_speed_control_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p); diff --git a/src/examples/flow_speed_control/module.mk b/src/examples/flow_speed_control/module.mk new file mode 100644 index 0000000000..5a41821460 --- /dev/null +++ b/src/examples/flow_speed_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build flow speed control +# + +MODULE_COMMAND = flow_speed_control + +SRCS = flow_speed_control_main.c \ + flow_speed_control_params.c 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 8e18c3c9a7..16d5ad6262 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -126,7 +126,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 12400, + 14000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f67cfad879..178986f610 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -241,7 +241,7 @@ int sdlog2_main(int argc, char *argv[]) deamon_task = task_spawn("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, - 2048, + 3000, sdlog2_thread_main, (const char **)argv); exit(0); @@ -661,6 +661,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int vicon_pos_sub; int flow_sub; int rc_sub; + int airspeed_sub; } subs; /* log message buffer: header + body */ @@ -680,6 +681,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; + struct log_AIRS_s log_AIRS; struct log_ARSP_s log_ARSP; } body; } log_msg = { @@ -784,6 +786,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- AIRSPEED --- */ + subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + fds[fdsc_count].fd = subs.airspeed_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1070,6 +1078,15 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(RC); } + /* --- AIRSPEED --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); + log_msg.msg_type = LOG_AIRS_MSG; + log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; + log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; + LOGBUFFER_WRITE_AND_COUNT(AIRS); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { #ifdef SDLOG2_DEBUG diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fc5687988c..efb999a509 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -172,6 +172,13 @@ struct log_OUT0_s { float output[8]; }; +/* --- AIRS - AIRSPEED --- */ +#define LOG_AIRS_MSG 13 +struct log_AIRS_s { + float indicated_airspeed; + float true_airspeed; +}; + /* --- ARSP - ATTITUDE RATE SET POINT --- */ #define LOG_ARSP_MSG 13 struct log_ARSP_s { @@ -196,6 +203,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), + LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), }; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2d..e22b58cad6 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -101,6 +101,9 @@ ORB_DEFINE(vehicle_command, struct vehicle_command_s); #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); +#include "topics/vehicle_bodyframe_speed_setpoint.h" +ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); + #include "topics/vehicle_global_position_setpoint.h" ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); @@ -119,6 +122,9 @@ ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); +#include "topics/filtered_bottom_flow.h" +ORB_DEFINE(filtered_bottom_flow, struct filtered_bottom_flow_s); + #include "topics/omnidirectional_flow.h" ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s); diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h new file mode 100644 index 0000000000..ab6de26131 --- /dev/null +++ b/src/modules/uORB/topics/filtered_bottom_flow.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 filtered_bottom_flow.h + * Definition of the filtered bottom flow uORB topic. + */ + +#ifndef TOPIC_FILTERED_BOTTOM_FLOW_H_ +#define TOPIC_FILTERED_BOTTOM_FLOW_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Filtered bottom flow in bodyframe. + */ +struct filtered_bottom_flow_s +{ + uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ + + float sumx; /**< Integrated bodyframe x flow in meters */ + float sumy; /**< Integrated bodyframe y flow in meters */ + + float vx; /**< Flow bodyframe x speed, m/s */ + float vy; /**< Flow bodyframe y Speed, m/s */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(filtered_bottom_flow); + +#endif diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h new file mode 100644 index 0000000000..fbfab09f30 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 vehicle_bodyframe_speed_setpoint.h + * Definition of the bodyframe speed setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ +#define TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_bodyframe_speed_setpoint_s +{ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + float vx; /**< in m/s */ + float vy; /**< in m/s */ +// float vz; /**< in m/s */ + float thrust_sp; + float yaw_sp; /**< in radian -PI +PI */ +}; /**< Speed in bodyframe to go to */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_bodyframe_speed_setpoint); + +#endif