dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'fmuv2_bringup_new_state_machine_drton' of https://github.com/cvg/Firmware_Private into fmuv2_bringup_new_state_machine_drton

This commit is contained in:
Simon Wilks 2013-08-19 18:03:58 +02:00
parent 44afdbcd65 7bd952c7ac
commit 32b6f62308
49 changed files with 2373 additions and 1106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -18,10 +18,15 @@ fi
ms5611 start
adc start
# mag might be external
if hmc5883 start
then
echo "using HMC5883"
fi
if mpu6000 start
then
echo "using MPU6000 and HMC5883L"
hmc5883 start
echo "using MPU6000"
set BOARD fmuv1
else
echo "using L3GD20 and LSM303D"

9
ROMFS/px4fmu_common/init.d/rcS
View File

@ -58,6 +58,15 @@ fi
if [ $MODE == autostart ]
then
#
# Start terminal
#
if sercon
then
echo "USB connected"
nshterm /dev/ttyACM0 &
fi
#
# Start the ORB (first app to start)
#

2
makefiles/config_px4fmu-v1_default.mk
View File

@ -56,6 +56,7 @@ MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
#
# General system control
@ -72,6 +73,7 @@ MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3_comp
MODULES += modules/position_estimator
MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator
#

2
makefiles/config_px4fmu-v2_default.mk
View File

@ -52,6 +52,8 @@ MODULES += systemcmds/pwm
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
#
# General system control

14
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -420,7 +420,7 @@ CONFIG_TASK_NAME_SIZE=24
CONFIG_START_YEAR=1970
CONFIG_START_MONTH=1
CONFIG_START_DAY=1
# CONFIG_DEV_CONSOLE is not set
CONFIG_DEV_CONSOLE=y
# CONFIG_MUTEX_TYPES is not set
CONFIG_PRIORITY_INHERITANCE=y
CONFIG_SEM_PREALLOCHOLDERS=8
@ -523,7 +523,7 @@ CONFIG_PIPES=y
# CONFIG_POWER is not set
# CONFIG_SENSORS is not set
CONFIG_SERIAL=y
# CONFIG_DEV_LOWCONSOLE is not set
CONFIG_DEV_LOWCONSOLE=y
CONFIG_SERIAL_REMOVABLE=y
# CONFIG_16550_UART is not set
CONFIG_ARCH_HAVE_UART4=y
@ -542,8 +542,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2
# CONFIG_UART4_SERIAL_CONSOLE is not set
# CONFIG_USART6_SERIAL_CONSOLE is not set
# CONFIG_UART7_SERIAL_CONSOLE is not set
# CONFIG_UART8_SERIAL_CONSOLE is not set
CONFIG_NO_SERIAL_CONSOLE=y
CONFIG_UART8_SERIAL_CONSOLE=y
# CONFIG_NO_SERIAL_CONSOLE is not set
#
# USART1 Configuration
@ -650,7 +650,7 @@ CONFIG_USBDEV_MAXPOWER=500
# CONFIG_USBDEV_COMPOSITE is not set
# CONFIG_PL2303 is not set
CONFIG_CDCACM=y
CONFIG_CDCACM_CONSOLE=y
CONFIG_CDCACM_CONSOLE=n
CONFIG_CDCACM_EP0MAXPACKET=64
CONFIG_CDCACM_EPINTIN=1
CONFIG_CDCACM_EPINTIN_FSSIZE=64
@ -716,10 +716,10 @@ CONFIG_FS_BINFS=y
#
# System Logging
#
# CONFIG_SYSLOG_ENABLE is not set
CONFIG_SYSLOG_ENABLE=y
CONFIG_SYSLOG=y
CONFIG_SYSLOG_CHAR=y
CONFIG_SYSLOG_DEVPATH="/dev/ttyS0"
CONFIG_SYSLOG_DEVPATH="/dev/ttyS6"
#
# Graphics Support

3
src/drivers/drv_mag.h
View File

@ -111,4 +111,7 @@ ORB_DECLARE(sensor_mag);
/** perform self test and report status */
#define MAGIOCSELFTEST _MAGIOC(7)
/** determine if mag is external or onboard */
#define MAGIOCGEXTERNAL _MAGIOC(8)
#endif /* _DRV_MAG_H */

21
src/drivers/hmc5883/hmc5883.cpp
View File

@ -167,6 +167,8 @@ private:
bool _sensor_ok; /**< sensor was found and reports ok */
bool _calibrated; /**< the calibration is valid */
int _bus; /**< the bus the device is connected to */
/**
* Test whether the device supported by the driver is present at a
* specific address.
@ -326,7 +328,8 @@ HMC5883::HMC5883(int bus) :
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
_sensor_ok(false),
_calibrated(false)
_calibrated(false),
_bus(bus)
{
// enable debug() calls
_debug_enabled = false;
@ -665,6 +668,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCSELFTEST:
return check_calibration();
case MAGIOCGEXTERNAL:
if (_bus == PX4_I2C_BUS_EXPANSION)
return 1;
else
return 0;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@ -851,8 +860,9 @@ HMC5883::collect()
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
} else {
#endif
/* XXX axis assignment of external sensor is yet unknown */
_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
* therefore switch and invert x and y */
_reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
@ -1293,6 +1303,11 @@ test()
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
/* check if mag is onboard or external */
if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0)
errx(1, "failed to get if mag is onboard or external");
warnx("device active: %s", ret ? "external" : "onboard");
/* set the queue depth to 10 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
errx(1, "failed to set queue depth");

15
src/drivers/lsm303d/lsm303d.cpp
View File

@ -869,6 +869,10 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
// return self_test();
return -EINVAL;
case MAGIOCGEXTERNAL:
/* no external mag board yet */
return 0;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
@ -1422,7 +1426,7 @@ test()
int fd_accel = -1;
struct accel_report accel_report;
ssize_t sz;
int filter_bandwidth;
int ret;
/* get the driver */
fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
@ -1445,10 +1449,10 @@ test()
warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
warnx("accel antialias filter bandwidth: fail");
else
warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth);
warnx("accel antialias filter bandwidth: %d Hz", ret);
int fd_mag = -1;
struct mag_report m_report;
@ -1459,6 +1463,11 @@ test()
if (fd_mag < 0)
err(1, "%s open failed", MAG_DEVICE_PATH);
/* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
errx(1, "failed to get if mag is onboard or external");
warnx("device active: %s", ret ? "external" : "onboard");
/* do a simple demand read */
sz = read(fd_mag, &m_report, sizeof(m_report));

5
src/drivers/rgbled/rgbled.cpp
View File

@ -443,7 +443,7 @@ void rgbled_usage();
void rgbled_usage() {
warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb'");
warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'");
warnx("options:");
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
errx(0, " -a addr (0x%x)", ADDR);
@ -534,7 +534,8 @@ rgbled_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(verb, "off")) {
if (!strcmp(verb, "stop") || !strcmp(verb, "off")) {
/* although technically it doesn't stop, this is the excepted syntax */
fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);

278
src/modules/commander/commander.cpp
View File

@ -118,12 +118,9 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define GPS_FIX_TYPE_2D 2
#define GPS_FIX_TYPE_3D 3
#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */
#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
#define RC_TIMEOUT 100000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 2000000
@ -201,12 +198,14 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
int commander_thread_main(int argc, char *argv[]);
void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
void print_reject_mode(const char *msg);
void print_reject_arm(const char *msg);
void print_status();
@ -399,27 +398,43 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM:
break;
case VEHICLE_CMD_NAV_TAKEOFF: {
transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
if (nav_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
}
if (nav_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
break;
}
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_safe(status, safety, armed)) {
if (is_safe(status, safety, armed)) {
if (((int)(cmd->param1)) == 1) {
/* reboot */
systemreset(false);
} else if (((int)(cmd->param1)) == 3) {
/* reboot to bootloader */
if (((int)(cmd->param1)) == 1) {
/* reboot */
systemreset(false);
} else if (((int)(cmd->param1)) == 3) {
/* reboot to bootloader */
systemreset(true);
// XXX implement
result = VEHICLE_CMD_RESULT_UNSUPPORTED;
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
break;
case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
@ -565,6 +580,7 @@ int commander_thread_main(int argc, char *argv[])
orb_advert_t status_pub;
/* make sure we are in preflight state */
memset(&status, 0, sizeof(status));
status.condition_landed = true; // initialize to safe value
/* armed topic */
struct actuator_armed_s armed;
@ -580,7 +596,7 @@ int commander_thread_main(int argc, char *argv[])
memset(&control_mode, 0, sizeof(control_mode));
status.main_state = MAIN_STATE_MANUAL;
status.navigation_state = NAVIGATION_STATE_STANDBY;
status.navigation_state = NAVIGATION_STATE_DIRECT;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
@ -743,9 +759,6 @@ int commander_thread_main(int argc, char *argv[])
start_time = hrt_absolute_time();
while (!thread_should_exit) {
hrt_abstime t = hrt_absolute_time();
status_changed = false;
/* update parameters */
orb_check(param_changed_sub, &updated);
@ -805,9 +818,10 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
last_diff_pres_time = diff_pres.timestamp;
}
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
orb_check(cmd_sub, &updated);
if (updated) {
@ -833,6 +847,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
}
/* update condition_global_position_valid */
check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
/* update local position estimate */
orb_check(local_position_sub, &updated);
@ -841,19 +858,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
}
/* set the condition to valid if there has recently been a local position estimate */
if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) {
if (!status.condition_local_position_valid) {
status.condition_local_position_valid = true;
status_changed = true;
}
} else {
if (status.condition_local_position_valid) {
status.condition_local_position_valid = false;
status_changed = true;
}
}
/* update condition_local_position_valid and condition_local_altitude_valid */
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
/* update battery status */
orb_check(battery_sub, &updated);
@ -861,8 +868,10 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
// warnx("bat v: %2.2f", battery.voltage_v);
/* only consider battery voltage if system has been running 2s and battery voltage is not 0 */
if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) {
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) {
status.battery_voltage = battery.voltage_v;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
@ -980,36 +989,39 @@ int commander_thread_main(int argc, char *argv[])
* set of position measurements is available.
*/
/* store current state to reason later about a state change */
// bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
// bool global_pos_valid = status.condition_global_position_valid;
// bool local_pos_valid = status.condition_local_position_valid;
// bool airspeed_valid = status.condition_airspeed_valid;
// <<<<<<< HEAD
// /* store current state to reason later about a state change */
// // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
// // bool global_pos_valid = status.condition_global_position_valid;
// // bool local_pos_valid = status.condition_local_position_valid;
// // bool airspeed_valid = status.condition_airspeed_valid;
/* check for global or local position updates, set a timeout of 2s */
if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) {
status.condition_global_position_valid = true;
// /* check for global or local position updates, set a timeout of 2s */
// if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) {
// status.condition_global_position_valid = true;
} else {
status.condition_global_position_valid = false;
}
// } else {
// status.condition_global_position_valid = false;
// }
if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) {
status.condition_local_position_valid = true;
// if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) {
// status.condition_local_position_valid = true;
} else {
status.condition_local_position_valid = false;
}
// } else {
// status.condition_local_position_valid = false;
// }
/* Check for valid airspeed/differential pressure measurements */
if (t - last_diff_pres_time < 2000000 && t > 2000000) {
status.condition_airspeed_valid = true;
// /* Check for valid airspeed/differential pressure measurements */
// if (t - last_diff_pres_time < 2000000 && t > 2000000) {
// status.condition_airspeed_valid = true;
} else {
status.condition_airspeed_valid = false;
}
// } else {
// status.condition_airspeed_valid = false;
// }
// =======
// >>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70
orb_check(gps_sub, &updated);
if (updated) {
@ -1033,10 +1045,9 @@ int commander_thread_main(int argc, char *argv[])
* position to the current position.
*/
if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D &&
if (!home_position_set && gps_position.fix_type >= 3 &&
(hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
(t - gps_position.timestamp_position < 2000000)
&& !armed.armed) {
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
/* copy position data to uORB home message, store it locally as well */
// TODO use global position estimate
home.lat = gps_position.lat;
@ -1071,7 +1082,7 @@ int commander_thread_main(int argc, char *argv[])
/* ignore RC signals if in offboard control mode */
if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
/* start RC input check */
if ((t - sp_man.timestamp) < 100000) {
if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
@ -1143,6 +1154,17 @@ int commander_thread_main(int argc, char *argv[])
} else {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates safety switch not pressed */
if (!(!safety.safety_switch_available || safety.safety_off)) {
print_reject_arm("NOT ARMING: Press safety switch first.");
} else {
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
}
}
/* fill current_status according to mode switches */
@ -1164,7 +1186,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
/* print error message for first RC glitch and then every 5s */
if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) {
if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) {
// TODO remove debug code
if (!status.rc_signal_cutting_off) {
warnx("Reason: not rc_signal_cutting_off\n");
@ -1177,11 +1199,11 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_cutting_off = true;
mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL");
last_print_control_time = t;
last_print_control_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
status.rc_signal_lost_interval = t - sp_man.timestamp;
status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
/* if the RC signal is gone for a full second, consider it lost */
if (status.rc_signal_lost_interval > 1000000) {
@ -1198,7 +1220,7 @@ int commander_thread_main(int argc, char *argv[])
// TODO check this
/* state machine update for offboard control */
if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) {
if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ?
if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ?
// /* decide about attitude control flag, enable in att/pos/vel */
// bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
@ -1254,23 +1276,23 @@ int commander_thread_main(int argc, char *argv[])
} else {
/* print error message for first offboard signal glitch and then every 5s */
if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) {
if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) {
status.offboard_control_signal_weak = true;
mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL");
last_print_control_time = t;
last_print_control_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp;
status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
/* if the signal is gone for 0.1 seconds, consider it lost */
if (status.offboard_control_signal_lost_interval > 100000) {
status.offboard_control_signal_lost = true;
status.failsave_lowlevel_start_time = t;
status.failsave_lowlevel_start_time = hrt_absolute_time();
tune_positive();
/* kill motors after timeout */
if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) {
status.failsave_lowlevel = true;
status_changed = true;
}
@ -1295,30 +1317,35 @@ int commander_thread_main(int argc, char *argv[])
if (arming_state_changed || main_state_changed || navigation_state_changed) {
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
status_changed = true;
} else {
status_changed = false;
}
hrt_abstime t1 = hrt_absolute_time();
/* publish arming state */
if (arming_state_changed) {
armed.timestamp = t;
armed.timestamp = t1;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
/* publish control mode */
if (navigation_state_changed) {
if (navigation_state_changed || arming_state_changed) {
/* publish new navigation state */
control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic
control_mode.counter++;
control_mode.timestamp = t;
control_mode.timestamp = t1;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
}
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
status.counter++;
status.timestamp = t;
status.timestamp = t1;
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
control_mode.timestamp = t;
control_mode.timestamp = t1;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
armed.timestamp = t;
armed.timestamp = t1;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@ -1387,6 +1414,18 @@ int commander_thread_main(int argc, char *argv[])
return 0;
}
void
check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
{
hrt_abstime t = hrt_absolute_time();
bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
if (*valid_out != valid_new) {
*valid_out = valid_new;
*changed = true;
}
}
void
toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
{
@ -1409,8 +1448,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
if (changed) {
warnx("changed");
int i;
rgbled_pattern_t pattern;
memset(&pattern, 0, sizeof(pattern));
@ -1607,37 +1644,54 @@ print_reject_mode(const char *msg)
}
}
void
print_reject_arm(const char *msg)
{
hrt_abstime t = hrt_absolute_time();
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t;
char s[80];
sprintf(s, "[cmd] %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative();
}
}
transition_result_t
check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode)
{
transition_result_t res = TRANSITION_DENIED;
if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
/* ARMED */
switch (current_status->main_state) {
case MAIN_STATE_MANUAL:
res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
break;
switch (current_status->main_state) {
case MAIN_STATE_MANUAL:
res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
break;
case MAIN_STATE_SEATBELT:
res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
break;
case MAIN_STATE_SEATBELT:
res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
break;
case MAIN_STATE_EASY:
res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
break;
case MAIN_STATE_EASY:
res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
break;
case MAIN_STATE_AUTO:
if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't act while taking off */
if (current_status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
// TRANSITION_DENIED is not possible here
break;
case MAIN_STATE_AUTO:
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't act while taking off */
res = TRANSITION_NOT_CHANGED;
} else {
/* if not landed: act depending on switches */
} else {
if (current_status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
// TRANSITION_DENIED is not possible here
break;
} else {
/* if not landed: */
if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (current_status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
@ -1652,18 +1706,18 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
} else {
/* always switch to loiter in air when no RC control */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
break;
default:
break;
}
} else {
/* DISARMED */
res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode);
break;
default:
break;
}
return res;

156
src/modules/commander/state_machine_helper.cpp
View File

@ -114,9 +114,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow arming from STANDBY and IN-AIR-RESTORE */
if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */
{
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = true;
@ -193,6 +192,7 @@ bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s
// 3) Safety switch is present AND engaged -> actuators locked
if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
return true;
} else {
return false;
}
@ -228,9 +228,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
case MAIN_STATE_SEATBELT:
/* need position estimate */
// TODO only need altitude estimate really
if (current_state->condition_local_position_valid) {
/* need altitude estimate */
if (current_state->condition_local_altitude_valid) {
ret = TRANSITION_CHANGED;
}
@ -238,7 +237,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
case MAIN_STATE_EASY:
/* need position estimate */
/* need local position estimate */
if (current_state->condition_local_position_valid) {
ret = TRANSITION_CHANGED;
}
@ -247,8 +246,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
case MAIN_STATE_AUTO:
/* need position estimate */
if (current_state->condition_local_position_valid) {
/* need global position estimate */
if (current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
@ -288,16 +287,6 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
} else {
switch (new_navigation_state) {
case NAVIGATION_STATE_STANDBY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = false;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_DIRECT:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
@ -305,6 +294,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = true;
break;
@ -315,6 +305,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = true;
break;
@ -325,6 +316,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = true;
break;
@ -335,22 +327,73 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_AUTO_READY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_rates_enabled = false;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_AUTO_TAKEOFF:
/* only transitions from AUTO_READY */
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LOITER:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_AUTO_RTL:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_AUTO_LAND:
/* deny transitions from landed state */
if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
@ -358,70 +401,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LOITER:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_MISSION:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_RTL:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LAND:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
}

4
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
View File

@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
}
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL);
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
/* Pitch (P) */
@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
/* set pitch plus feedforward roll compensation */
rates_sp->pitch = pid_calculate(&pitch_controller,
att_sp->pitch_body + pitch_sp_rollcompensation,
att->pitch, 0, 0, NULL, NULL, NULL);
att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))

166
src/modules/fixedwing_att_control/fixedwing_att_control_main.c
View File

@ -53,6 +53,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
@ -116,6 +117,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
@ -137,7 +140,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
@ -178,96 +182,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* control */
/* set manual setpoints if required */
if (control_mode.flag_control_manual_enabled) {
if (control_mode.flag_control_attitude_enabled) {
#warning fix this
// if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
// vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
// /* attitude control */
// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
//
// /* angular rate control */
// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//
// /* pass through throttle */
// actuators.control[3] = att_sp.thrust;
//
// /* set flaps to zero */
// actuators.control[4] = 0.0f;
//
// } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
//
// /* if the RC signal is lost, try to stay level and go slowly back down to ground */
// if (vstatus.rc_signal_lost) {
//
// /* put plane into loiter */
// att_sp.roll_body = 0.3f;
// att_sp.pitch_body = 0.0f;
//
// /* limit throttle to 60 % of last value if sane */
// if (isfinite(manual_sp.throttle) &&
// (manual_sp.throttle >= 0.0f) &&
// (manual_sp.throttle <= 1.0f)) {
// att_sp.thrust = 0.6f * manual_sp.throttle;
//
// } else {
// att_sp.thrust = 0.0f;
// }
//
// att_sp.yaw_body = 0;
//
// // XXX disable yaw control, loiter
//
// } else {
//
// att_sp.roll_body = manual_sp.roll;
// att_sp.pitch_body = manual_sp.pitch;
// att_sp.yaw_body = 0;
// att_sp.thrust = manual_sp.throttle;
// }
//
// att_sp.timestamp = hrt_absolute_time();
//
// /* attitude control */
// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
//
// /* angular rate control */
// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//
// /* pass through throttle */
// actuators.control[3] = att_sp.thrust;
//
// /* pass through flaps */
// if (isfinite(manual_sp.flaps)) {
// actuators.control[4] = manual_sp.flaps;
//
// } else {
// actuators.control[4] = 0.0f;
// }
//
// } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
// /* directly pass through values */
// actuators.control[0] = manual_sp.roll;
// /* positive pitch means negative actuator -> pull up */
// actuators.control[1] = manual_sp.pitch;
// actuators.control[2] = manual_sp.yaw;
// actuators.control[3] = manual_sp.throttle;
//
// if (isfinite(manual_sp.flaps)) {
// actuators.control[4] = manual_sp.flaps;
//
// } else {
// actuators.control[4] = 0.0f;
// }
// }
// }
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
/* execute attitude control if requested */
if (control_mode.flag_control_attitude_enabled) {
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
}
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);

6
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
View File

@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
/* roll rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL);
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
/* pitch rate (PI) */
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL);
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
/* yaw rate (PI) */
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL);
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
counter++;

171
src/modules/fixedwing_backside/fixedwing.cpp
View File

@ -39,8 +39,6 @@
#include "fixedwing.hpp"
#if 0
namespace control
{
@ -157,9 +155,8 @@ void BlockMultiModeBacksideAutopilot::update()
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
#warning this if is incomplete, should be based on flags
// only update guidance in auto mode
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
}
@ -168,10 +165,9 @@ void BlockMultiModeBacksideAutopilot::update()
// once the system switches from manual or auto to stabilized
// the setpoint should update to loitering around this position
#warning should be base on flags
// handle autopilot modes
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
@ -223,93 +219,83 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
/* do not limit in HIL */
#warning fix this
// if (!_status.flag_hil_enabled) {
// /* limit to value of manual throttle */
// _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
// _actuators.control[CH_THR] : _manual.throttle;
// }
} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
#warning fix here too
// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
// _actuators.control[CH_AIL] = _manual.roll;
// _actuators.control[CH_ELV] = _manual.pitch;
// _actuators.control[CH_RDR] = _manual.yaw;
// _actuators.control[CH_THR] = _manual.throttle;
//
// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
// from +1 -> -1 m/s for rate of climb
//float dThrottle = _cr2Thr.update(
//_crMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
float pCmd = _phi2P.update(phiCmd - _att.roll);
// throttle channel -> velocity
// negative sign because nose over to increase speed
float vCmd = _vLimit.update(_manual.throttle *
(_vLimit.getMax() - _vLimit.getMin()) +
_vLimit.getMin());
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
// currently using manual throttle
// XXX if you enable this watch out, vz might be very noisy
//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.control[CH_THR] = _manual.throttle;
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
// a first binary release can be targeted.
// This is not a hack, but a design choice.
/* do not limit in HIL */
#warning fix this
// if (!_status.flag_hil_enabled) {
// /* limit to value of manual throttle */
// _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
// _actuators.control[CH_THR] : _manual.throttle;
// }
#warning fix this
// }
// body rates controller, disabled for now
else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
if (_status.hil_state != HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
} else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here?
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
// from +1 -> -1 m/s for rate of climb
//float dThrottle = _cr2Thr.update(
//_crMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
float pCmd = _phi2P.update(phiCmd - _att.roll);
// throttle channel -> velocity
// negative sign because nose over to increase speed
float vCmd = _vLimit.update(_manual.throttle *
(_vLimit.getMax() - _vLimit.getMin()) +
_vLimit.getMin());
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
// currently using manual throttle
// XXX if you enable this watch out, vz might be very noisy
//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.control[CH_THR] = _manual.throttle;
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
// a first binary release can be targeted.
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (_status.hil_state != HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
// body rates controller, disabled for now
// TODO
} else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here?
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
}
// update all publications
@ -330,4 +316,3 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
} // namespace control
#endif

6
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
View File

@ -151,14 +151,12 @@ int control_demo_thread_main(int argc, char *argv[])
using namespace control;
#warning fix this
// fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
thread_running = true;
while (!thread_should_exit) {
#warning fix this
// autopilot.update();
autopilot.update();
}
warnx("exiting.");

10
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
View File

@ -271,7 +271,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F); //TODO: remove hardcoded value
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
}
/* only run controller if attitude changed */
@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
// XXX what is xtrack_err.past_end?
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
}
/* calculate roll setpoint, do this artificially around zero */
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL);
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL);
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
if (verbose) {
printf("psi_rate_c %.4f ", (double)psi_rate_c);
@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
if (pos_updated) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL);
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}

229
src/modules/multirotor_att_control/multirotor_att_control_main.c
View File

@ -58,13 +58,11 @@
#include <uORB/uORB.h>
#include <drivers/drv_gyro.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_debug.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
@ -84,18 +82,12 @@ static bool thread_should_exit;
static int mc_task;
static bool motor_test_mode = false;
static orb_advert_t actuator_pub;
static orb_advert_t control_debug_pub;
static int
mc_thread_main(int argc, char *argv[])
{
/* declare and safely initialize all structs */
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
@ -112,16 +104,12 @@ mc_thread_main(int argc, char *argv[])
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
struct vehicle_control_debug_s control_debug;
memset(&control_debug, 0, sizeof(control_debug));
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@ -142,10 +130,8 @@ mc_thread_main(int argc, char *argv[])
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug);
actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
@ -156,25 +142,20 @@ mc_thread_main(int argc, char *argv[])
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
/* welcome user */
printf("[multirotor_att_control] starting\n");
warnx("starting");
/* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
bool flag_armed = false;
bool flag_control_position_enabled = false;
bool flag_control_velocity_enabled = false;
/* store if yaw position or yaw speed has been changed */
bool control_yaw_position = true;
/* store if we stopped a yaw movement */
bool first_time_after_yaw_speed_control = true;
bool reset_yaw_sp = true;
/* prepare the handle for the failsafe throttle */
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
float failsafe_throttle = 0.0f;
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
@ -187,6 +168,7 @@ mc_thread_main(int argc, char *argv[])
} else if (ret == 0) {
/* no return value, ignore */
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
@ -210,12 +192,6 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
orb_check(armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
@ -232,7 +208,7 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/** STEP 1: Define which input is the dominating control input */
/* define which input is the dominating control input */
if (control_mode.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
@ -240,7 +216,6 @@ mc_thread_main(int argc, char *argv[])
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
// printf("thrust_rate=%8.4f\n",offboard_sp.p4);
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
@ -249,58 +224,97 @@ mc_thread_main(int argc, char *argv[])
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
// printf("thrust_att=%8.4f\n",offboard_sp.p4);
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else if (control_mode.flag_control_manual_enabled) {
} else if (control_mode.flag_control_manual_enabled) {
/* direct manual input */
if (control_mode.flag_control_attitude_enabled) {
/* control attitude, update attitude setpoint depending on mode */
/* initialize to current yaw if switching to manual or att control */
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
control_mode.flag_control_manual_enabled != flag_control_manual_enabled ||
armed.armed != flag_armed) {
control_mode.flag_control_manual_enabled != flag_control_manual_enabled) {
att_sp.yaw_body = att.yaw;
}
static bool rc_loss_first_time = true;
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (control_mode.failsave_highlevel) {
if (!control_mode.flag_control_velocity_enabled) {
/* Don't reset attitude setpoint in position control mode, it's handled by position controller. */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
rc_loss_first_time = true;
if (!control_mode.flag_control_climb_rate_enabled) {
/* Don't touch throttle in modes with altitude hold, it's handled by position controller.
*
* Only go to failsafe throttle if last known throttle was
* high enough to create some lift to make hovering state likely.
*
* This is to prevent that someone landing, but not disarming his
* multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote.
*/
if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.thrust = failsafe_throttle;
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
/* set attitude if arming */
if (!flag_control_attitude_enabled && armed.armed) {
att_sp.yaw_body = att.yaw;
}
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
first_time_after_yaw_speed_control = true;
} else {
if (first_time_after_yaw_speed_control) {
att_sp.yaw_body = att.yaw;
first_time_after_yaw_speed_control = false;
} else {
att_sp.thrust = 0.0f;
}
}
}
control_yaw_position = true;
/* keep current yaw, do not attempt to go to north orientation,
* since if the pilot regains RC control, he will be lost regarding
* the current orientation.
*/
if (rc_loss_first_time)
att_sp.yaw_body = att.yaw;
rc_loss_first_time = false;
} else {
rc_loss_first_time = true;
/* control yaw in all manual / assisted modes */
/* set yaw if arming or switching to attitude stabilized mode */
if (!flag_control_attitude_enabled) {
reset_yaw_sp = true;
}
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
reset_yaw_sp = true;
} else {
if (reset_yaw_sp) {
att_sp.yaw_body = att.yaw;
reset_yaw_sp = false;
}
control_yaw_position = true;
}
if (!control_mode.flag_control_velocity_enabled) {
/* don't update attitude setpoint in position control mode */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
if (!control_mode.flag_control_climb_rate_enabled) {
/* don't set throttle in altitude hold modes */
att_sp.thrust = manual.throttle;
}
}
att_sp.timestamp = hrt_absolute_time();
}
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
@ -308,72 +322,71 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
/* STEP 2: publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else {
//XXX TODO add acro mode here
/* manual rate inputs, from RC control or joystick */
// if (state.flag_control_rates_enabled &&
// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
// rates_sp.roll = manual.roll;
//
// rates_sp.pitch = manual.pitch;
// rates_sp.yaw = manual.yaw;
// rates_sp.thrust = manual.throttle;
// rates_sp.timestamp = hrt_absolute_time();
// }
/* manual rate inputs (ACRO), from RC control or joystick */
if (control_mode.flag_control_rates_enabled) {
rates_sp.roll = manual.roll;
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
}
}
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
if (control_mode.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, &control_debug_pub, &control_debug);
/* check if we should we reset integrals */
bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle
/* run attitude controller if needed */
if (control_mode.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
float rates[3];
float rates_acc[3];
/* run rates controller if needed */
if (control_mode.flag_control_rates_enabled) {
/* get current rate setpoint */
bool rates_sp_updated = false;
orb_check(rates_sp_sub, &rates_sp_updated);
/* get current rate setpoint */
bool rates_sp_valid = false;
orb_check(rates_sp_sub, &rates_sp_valid);
if (rates_sp_updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
}
if (rates_sp_valid) {
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
/* apply controller */
float rates[3];
rates[0] = att.rollspeed;
rates[1] = att.pitchspeed;
rates[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
} else {
/* rates controller disabled, set actuators to zero for safety */
actuators.control[0] = 0.0f;
actuators.control[1] = 0.0f;
actuators.control[2] = 0.0f;
actuators.control[3] = 0.0f;
}
/* apply controller */
rates[0] = att.rollspeed;
rates[1] = att.pitchspeed;
rates[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, rates, &actuators, &control_debug_pub, &control_debug);
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug);
/* update control_mode */
/* update state */
flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
flag_control_position_enabled = control_mode.flag_control_position_enabled;
flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled;
flag_armed = armed.armed;
perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */
} /* end of poll return value check */
}
printf("[multirotor att control] stopping, disarming motors.\n");
warnx("stopping, disarming motors");
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
@ -440,11 +453,11 @@ int multirotor_att_control_main(int argc, char *argv[])
thread_should_exit = false;
mc_task = task_spawn_cmd("multirotor_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
2048,
mc_thread_main,
NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
2048,
mc_thread_main,
NULL);
exit(0);
}

53
src/modules/multirotor_att_control/multirotor_attitude_control.c
View File

@ -65,29 +65,50 @@
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
struct mc_att_control_params {
float yaw_p;
float yaw_i;
float yaw_d;
//float yaw_awu;
//float yaw_lim;
float att_p;
float att_i;
float att_d;
//float att_awu;
//float att_lim;
//float att_xoff;
//float att_yoff;
};
struct mc_att_control_param_handles {
param_t yaw_p;
param_t yaw_i;
param_t yaw_d;
//param_t yaw_awu;
//param_t yaw_lim;
param_t att_p;
param_t att_i;
param_t att_d;
//param_t att_awu;
//param_t att_lim;
//param_t att_xoff;
//param_t att_yoff;
};
/**
@ -109,10 +130,17 @@ static int parameters_init(struct mc_att_control_param_handles *h)
h->yaw_p = param_find("MC_YAWPOS_P");
h->yaw_i = param_find("MC_YAWPOS_I");
h->yaw_d = param_find("MC_YAWPOS_D");
//h->yaw_awu = param_find("MC_YAWPOS_AWU");
//h->yaw_lim = param_find("MC_YAWPOS_LIM");
h->att_p = param_find("MC_ATT_P");
h->att_i = param_find("MC_ATT_I");
h->att_d = param_find("MC_ATT_D");
//h->att_awu = param_find("MC_ATT_AWU");
//h->att_lim = param_find("MC_ATT_LIM");
//h->att_xoff = param_find("MC_ATT_XOFF");
//h->att_yoff = param_find("MC_ATT_YOFF");
return OK;
}
@ -122,17 +150,23 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
param_get(h->yaw_p, &(p->yaw_p));
param_get(h->yaw_i, &(p->yaw_i));
param_get(h->yaw_d, &(p->yaw_d));
//param_get(h->yaw_awu, &(p->yaw_awu));
//param_get(h->yaw_lim, &(p->yaw_lim));
param_get(h->att_p, &(p->att_p));
param_get(h->att_i, &(p->att_i));
param_get(h->att_d, &(p->att_d));
//param_get(h->att_awu, &(p->att_awu));
//param_get(h->att_lim, &(p->att_lim));
//param_get(h->att_xoff, &(p->att_xoff));
//param_get(h->att_yoff, &(p->att_yoff));
return OK;
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
@ -173,30 +207,29 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* apply parameters */
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
}
/* reset integral if on ground */
if (att_sp->thrust < 0.1f) {
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_controller);
pid_reset_integral(&roll_controller);
//TODO pid_reset_integral(&yaw_controller);
}
/* calculate current control outputs */
/* control pitch (forward) output */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
att->pitch, att->pitchspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d);
att->pitch, att->pitchspeed, deltaT);
/* control roll (left/right) output */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
att->roll, att->rollspeed, deltaT, NULL, NULL, NULL);
// printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT);
att->roll, att->rollspeed, deltaT);
if (control_yaw_position) {
/* control yaw rate */
// TODO use pid lib
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);

5
src/modules/multirotor_att_control/multirotor_attitude_control.h
View File

@ -58,11 +58,8 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_debug.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */

22
src/modules/multirotor_att_control/multirotor_rate_control.c
View File

@ -58,9 +58,6 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
@ -155,8 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
}
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@ -176,13 +172,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
float pitch_control_last = 0.0f;
float roll_control_last = 0.0f;
static bool initialized = false;
float diff_filter_factor = 1.0f;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
@ -202,21 +193,20 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
}
/* reset integral if on ground */
if (rate_sp->thrust < 0.01f) {
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
// TODO pid_reset_integral(&yaw_rate_controller);
}
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
rates[1], 0.0f, deltaT,
&control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d);
rates[1], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
rates[0], 0.0f, deltaT,
&control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d);
rates[0], 0.0f, deltaT);
/* control yaw rate */ //XXX use library here
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);

4
src/modules/multirotor_att_control/multirotor_rate_control.h
View File

@ -57,10 +57,8 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_debug.h>
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */

3
src/modules/multirotor_pos_control/module.mk
View File

@ -38,4 +38,5 @@
MODULE_COMMAND = multirotor_pos_control
SRCS = multirotor_pos_control.c \
multirotor_pos_control_params.c
multirotor_pos_control_params.c \
thrust_pid.c

422
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -35,13 +35,14 @@
/**
* @file multirotor_pos_control.c
*
* Skeleton for multirotor position controller
* Multirotor position controller
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
@ -52,15 +53,21 @@
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <systemlib/systemlib.h>
#include <systemlib/pid/pid.h>
#include <mavlink/mavlink_log.h>
#include "multirotor_pos_control_params.h"
#include "thrust_pid.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
@ -79,12 +86,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]);
*/
static void usage(const char *reason);
static void
usage(const char *reason)
static float scale_control(float ctl, float end, float dz);
static float norm(float x, float y);
static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
exit(1);
}
@ -92,9 +103,9 @@ usage(const char *reason)
* 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_cmd().
* to task_spawn().
*/
int multirotor_pos_control_main(int argc, char *argv[])
{
@ -104,32 +115,36 @@ int multirotor_pos_control_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("multirotor pos control already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
warnx("start");
thread_should_exit = false;
deamon_task = task_spawn_cmd("multirotor pos control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60,
4096,
multirotor_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
deamon_task = task_spawn_cmd("multirotor_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60,
4096,
multirotor_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
warnx("stop");
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tmultirotor pos control app is running\n");
warnx("app is running");
} else {
printf("\tmultirotor pos control app not started\n");
warnx("app not started");
}
exit(0);
}
@ -137,98 +152,349 @@ int multirotor_pos_control_main(int argc, char *argv[])
exit(1);
}
static int
multirotor_pos_control_thread_main(int argc, char *argv[])
static float scale_control(float ctl, float end, float dz)
{
if (ctl > dz) {
return (ctl - dz) / (end - dz);
} else if (ctl < -dz) {
return (ctl + dz) / (end - dz);
} else {
return 0.0f;
}
}
static float norm(float x, float y)
{
return sqrtf(x * x + y * y);
}
static int multirotor_pos_control_thread_main(int argc, char *argv[])
{
/* welcome user */
printf("[multirotor pos control] Control started, taking over position control\n");
warnx("started");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[mpc] started");
/* structures */
struct vehicle_status_s state;
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
//struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_vicon_position_s local_pos;
struct manual_control_setpoint_s manual;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
/* subscribe to attitude, motor setpoints and system state */
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* publish attitude setpoint */
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
bool reset_sp_alt = true;
bool reset_sp_pos = true;
hrt_abstime t_prev = 0;
/* integrate in NED frame to estimate wind but not attitude offset */
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float home_alt = 0.0f;
hrt_abstime home_alt_t = 0;
uint64_t local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
PID_t xy_vel_pids[2];
PID_t z_pos_pid;
thrust_pid_t z_vel_pid;
thread_running = true;
int loopcounter = 0;
struct multirotor_position_control_params params;
struct multirotor_position_control_param_handles params_h;
parameters_init(&params_h);
parameters_update(&params_h, &params);
struct multirotor_position_control_params p;
struct multirotor_position_control_param_handles h;
parameters_init(&h);
parameters_update(&h, &p);
for (int i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
}
pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
while (1) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
/* get a local copy of local position setpoint */
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
int paramcheck_counter = 0;
bool global_pos_sp_updated = false;
if (loopcounter == 500) {
parameters_update(&h, &p);
loopcounter = 0;
while (!thread_should_exit) {
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
/* check parameters at 1 Hz */
if (++paramcheck_counter >= 50) {
paramcheck_counter = 0;
bool param_updated;
orb_check(param_sub, &param_updated);
if (param_updated) {
parameters_update(&params_h, &params);
for (int i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
/* use integral_limit_out = tilt_max / 2 */
float i_limit;
if (params.xy_vel_i == 0.0) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0;
} else {
i_limit = 1.0f; // not used really
}
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
}
}
// if (state.state_machine == SYSTEM_STATE_AUTO) {
// XXX IMPLEMENT POSITION CONTROL HERE
/* only check global position setpoint updates but not read */
if (!global_pos_sp_updated) {
orb_check(global_pos_sp_sub, &global_pos_sp_updated);
}
float dT = 1.0f / 50.0f;
hrt_abstime t = hrt_absolute_time();
float dt;
float x_setpoint = 0.0f;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
// XXX enable switching between Vicon and local position estimate
/* local pos is the Vicon position */
} else {
dt = 0.0f;
}
// XXX just an example, lacks rotation around world-body transformation
att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p;
att_sp.roll_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.3f;
att_sp.timestamp = hrt_absolute_time();
t_prev = t;
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
/* set setpoint to current position */
// XXX select pos reset channel on remote
/* reset setpoint to current position (position hold) */
// if (1 == 2) {
// local_pos_sp.x = local_pos.x;
// local_pos_sp.y = local_pos.y;
// local_pos_sp.z = local_pos.z;
// local_pos_sp.yaw = att.yaw;
// }
// }
if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
if (control_mode.flag_control_manual_enabled) {
/* manual mode, reset setpoints if needed */
if (reset_sp_alt) {
reset_sp_alt = false;
local_pos_sp.z = local_pos.z;
thrust_pid_set_integral(&z_vel_pid, -manual.throttle); // thrust PID uses Z downside
mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
}
if (control_mode.flag_control_position_enabled && reset_sp_pos) {
reset_sp_pos = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y);
}
} else {
/* non-manual mode, project global setpoints to local frame */
//orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
if (local_pos.ref_timestamp != local_ref_timestamp) {
local_ref_timestamp = local_pos.ref_timestamp;
/* init local projection using local position home */
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home);
mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home);
}
if (global_pos_sp_updated) {
global_pos_sp_updated = false;
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
double sp_lat = global_pos_sp.lat * 1e-7;
double sp_lon = global_pos_sp.lon * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
/* publish local position setpoint as projection of global position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
}
}
float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
/* manual control - move setpoints */
if (control_mode.flag_control_manual_enabled) {
if (local_pos.ref_timestamp != home_alt_t) {
if (home_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - home_alt;
}
home_alt_t = local_pos.ref_timestamp;
home_alt = local_pos.ref_alt;
}
if (control_mode.flag_control_altitude_enabled) {
/* move altitude setpoint with manual controls */
float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
if (z_sp_ctl != 0.0f) {
sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
local_pos_sp.z += sp_move_rate[2] * dt;
if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
local_pos_sp.z = local_pos.z + z_sp_offs_max;
} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
local_pos_sp.z = local_pos.z - z_sp_offs_max;
}
}
}
if (control_mode.flag_control_position_enabled) {
/* move position setpoint with manual controls */
float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
local_pos_sp.x += sp_move_rate[0] * dt;
local_pos_sp.y += sp_move_rate[1] * dt;
/* limit maximum setpoint from position offset and preserve direction
* fail safe, should not happen in normal operation */
float pos_vec_x = local_pos_sp.x - local_pos.x;
float pos_vec_y = local_pos_sp.y - local_pos.y;
float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
if (pos_vec_norm > 1.0f) {
local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
}
}
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
}
/* run position & altitude controllers, calculate velocity setpoint */
if (control_mode.flag_control_altitude_enabled) {
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
} else {
global_vel_sp.vz = 0.0f;
}
if (control_mode.flag_control_position_enabled) {
/* calculate velocity set point in NED frame */
global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
/* limit horizontal speed */
float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
if (xy_vel_sp_norm > 1.0f) {
global_vel_sp.vx /= xy_vel_sp_norm;
global_vel_sp.vy /= xy_vel_sp_norm;
}
} else {
reset_sp_pos = true;
global_vel_sp.vx = 0.0f;
global_vel_sp.vy = 0.0f;
}
/* publish new velocity setpoint */
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
// TODO subscribe to velocity setpoint if altitude/position control disabled
if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
bool reset_integral = !control_mode.flag_armed;
if (control_mode.flag_control_climb_rate_enabled) {
if (reset_integral) {
thrust_pid_set_integral(&z_vel_pid, params.thr_min);
}
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
att_sp.thrust = -thrust_sp[2];
}
if (control_mode.flag_control_velocity_enabled) {
/* calculate thrust set point in NED frame */
if (reset_integral) {
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
}
thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
/* limit horizontal part of thrust */
float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
/* assuming that vertical component of thrust is g,
* horizontal component = g * tan(alpha) */
float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
if (tilt > params.tilt_max) {
tilt = params.tilt_max;
}
/* convert direction to body frame */
thrust_xy_dir -= att.yaw;
/* calculate roll and pitch */
att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
}
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else {
reset_sp_alt = true;
reset_sp_pos = true;
}
/* run at approximately 50 Hz */
usleep(20000);
loopcounter++;
}
printf("[multirotor pos control] ending now...\n");
warnx("stopped");
mavlink_log_info(mavlink_fd, "[mpc] stopped");
thread_running = false;

63
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
View File

@ -34,29 +34,76 @@
****************************************************************************/
/*
* @file multirotor_position_control_params.c
* @file multirotor_pos_control_params.c
*
* Parameters for EKF filter
* Parameters for multirotor_pos_control
*/
#include "multirotor_pos_control_params.h"
/* Extended Kalman Filter covariances */
/* controller parameters */
PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f);
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f);
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
int parameters_init(struct multirotor_position_control_param_handles *h)
{
/* PID parameters */
h->p = param_find("MC_POS_P");
h->thr_min = param_find("MPC_THR_MIN");
h->thr_max = param_find("MPC_THR_MAX");
h->z_p = param_find("MPC_Z_P");
h->z_d = param_find("MPC_Z_D");
h->z_vel_p = param_find("MPC_Z_VEL_P");
h->z_vel_i = param_find("MPC_Z_VEL_I");
h->z_vel_d = param_find("MPC_Z_VEL_D");
h->z_vel_max = param_find("MPC_Z_VEL_MAX");
h->xy_p = param_find("MPC_XY_P");
h->xy_d = param_find("MPC_XY_D");
h->xy_vel_p = param_find("MPC_XY_VEL_P");
h->xy_vel_i = param_find("MPC_XY_VEL_I");
h->xy_vel_d = param_find("MPC_XY_VEL_D");
h->xy_vel_max = param_find("MPC_XY_VEL_MAX");
h->tilt_max = param_find("MPC_TILT_MAX");
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 multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
{
param_get(h->p, &(p->p));
param_get(h->thr_min, &(p->thr_min));
param_get(h->thr_max, &(p->thr_max));
param_get(h->z_p, &(p->z_p));
param_get(h->z_d, &(p->z_d));
param_get(h->z_vel_p, &(p->z_vel_p));
param_get(h->z_vel_i, &(p->z_vel_i));
param_get(h->z_vel_d, &(p->z_vel_d));
param_get(h->z_vel_max, &(p->z_vel_max));
param_get(h->xy_p, &(p->xy_p));
param_get(h->xy_d, &(p->xy_d));
param_get(h->xy_vel_p, &(p->xy_vel_p));
param_get(h->xy_vel_i, &(p->xy_vel_i));
param_get(h->xy_vel_d, &(p->xy_vel_d));
param_get(h->xy_vel_max, &(p->xy_vel_max));
param_get(h->tilt_max, &(p->tilt_max));
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;
}

44
src/modules/multirotor_pos_control/multirotor_pos_control_params.h
View File

@ -42,15 +42,47 @@
#include <systemlib/param/param.h>
struct multirotor_position_control_params {
float p;
float i;
float d;
float thr_min;
float thr_max;
float z_p;
float z_d;
float z_vel_p;
float z_vel_i;
float z_vel_d;
float z_vel_max;
float xy_p;
float xy_d;
float xy_vel_p;
float xy_vel_i;
float xy_vel_d;
float xy_vel_max;
float tilt_max;
float rc_scale_pitch;
float rc_scale_roll;
float rc_scale_yaw;
};
struct multirotor_position_control_param_handles {
param_t p;
param_t i;
param_t d;
param_t thr_min;
param_t thr_max;
param_t z_p;
param_t z_d;
param_t z_vel_p;
param_t z_vel_i;
param_t z_vel_d;
param_t z_vel_max;
param_t xy_p;
param_t xy_d;
param_t xy_vel_p;
param_t xy_vel_i;
param_t xy_vel_d;
param_t xy_vel_max;
param_t tilt_max;
param_t rc_scale_pitch;
param_t rc_scale_roll;
param_t rc_scale_yaw;
};
/**

235
src/modules/multirotor_pos_control/position_control.c
View File

@ -1,235 +0,0 @@
// /****************************************************************************
// *
// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
// * Author: @author Lorenz Meier <lm@inf.ethz.ch>
// * @author Laurens Mackay <mackayl@student.ethz.ch>
// * @author Tobias Naegeli <naegelit@student.ethz.ch>
// * @author Martin Rutschmann <rutmarti@student.ethz.ch>
// *
// * Redistribution and use in source and binary forms, with or without
// * modification, are permitted provided that the following conditions
// * are met:
// *
// * 1. Redistributions of source code must retain the above copyright
// * notice, this list of conditions and the following disclaimer.
// * 2. Redistributions in binary form must reproduce the above copyright
// * notice, this list of conditions and the following disclaimer in
// * the documentation and/or other materials provided with the
// * distribution.
// * 3. Neither the name PX4 nor the names of its contributors may be
// * used to endorse or promote products derived from this software
// * without specific prior written permission.
// *
// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
// * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
// * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// * POSSIBILITY OF SUCH DAMAGE.
// *
// ****************************************************************************/
// /**
// * @file multirotor_position_control.c
// * Implementation of the position control for a multirotor VTOL
// */
// #include <stdio.h>
// #include <stdlib.h>
// #include <stdio.h>
// #include <stdint.h>
// #include <math.h>
// #include <stdbool.h>
// #include <float.h>
// #include <systemlib/pid/pid.h>
// #include "multirotor_position_control.h"
// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
// {
// static PID_t distance_controller;
// static int read_ret;
// static global_data_position_t position_estimated;
// static uint16_t counter;
// static bool initialized;
// static uint16_t pm_counter;
// static float lat_next;
// static float lon_next;
// static float pitch_current;
// static float thrust_total;
// if (initialized == false) {
// pid_init(&distance_controller,
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
// PID_MODE_DERIVATIV_CALC, 150);//150
// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
// thrust_total = 0.0f;
// /* Position initialization */
// /* Wait for new position estimate */
// do {
// read_ret = read_lock_position(&position_estimated);
// } while (read_ret != 0);
// lat_next = position_estimated.lat;
// lon_next = position_estimated.lon;
// /* attitude initialization */
// global_data_lock(&global_data_attitude->access_conf);
// pitch_current = global_data_attitude->pitch;
// global_data_unlock(&global_data_attitude->access_conf);
// initialized = true;
// }
// /* load new parameters with 10Hz */
// if (counter % 50 == 0) {
// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
// /* check whether new parameters are available */
// if (global_data_parameter_storage->counter > pm_counter) {
// pid_set_parameters(&distance_controller,
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
// //
// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
// pm_counter = global_data_parameter_storage->counter;
// printf("Position controller changed pid parameters\n");
// }
// }
// global_data_unlock(&global_data_parameter_storage->access_conf);
// }
// /* Wait for new position estimate */
// do {
// read_ret = read_lock_position(&position_estimated);
// } while (read_ret != 0);
// /* Get next waypoint */ //TODO: add local copy
// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
// lat_next = global_data_position_setpoint->x;
// lon_next = global_data_position_setpoint->y;
// global_data_unlock(&global_data_position_setpoint->access_conf);
// }
// /* Get distance to waypoint */
// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// // if(counter % 5 == 0)
// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
// /* Get bearing to waypoint (direction on earth surface to next waypoint) */
// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
// if (counter % 5 == 0)
// printf("bearing: %.4f\n", bearing);
// /* Calculate speed in direction of bearing (needed for controller) */
// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// // if(counter % 5 == 0)
// // printf("speed_norm: %.4f\n", speed_norm);
// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
// /* Control Thrust in bearing direction */
// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// // if(counter % 5 == 0)
// // printf("horizontal thrust: %.4f\n", horizontal_thrust);
// /* Get total thrust (from remote for now) */
// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
// global_data_unlock(&global_data_rc_channels->access_conf);
// }
// const float max_gas = 500.0f;
// thrust_total *= max_gas / 20000.0f; //TODO: check this
// thrust_total += max_gas / 2.0f;
// if (horizontal_thrust > thrust_total) {
// horizontal_thrust = thrust_total;
// } else if (horizontal_thrust < -thrust_total) {
// horizontal_thrust = -thrust_total;
// }
// //TODO: maybe we want to add a speed controller later...
// /* Calclulate thrust in east and north direction */
// float thrust_north = cosf(bearing) * horizontal_thrust;
// float thrust_east = sinf(bearing) * horizontal_thrust;
// if (counter % 10 == 0) {
// printf("thrust north: %.4f\n", thrust_north);
// printf("thrust east: %.4f\n", thrust_east);
// fflush(stdout);
// }
// /* Get current attitude */
// if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
// pitch_current = global_data_attitude->pitch;
// global_data_unlock(&global_data_attitude->access_conf);
// }
// /* Get desired pitch & roll */
// float pitch_desired = 0.0f;
// float roll_desired = 0.0f;
// if (thrust_total != 0) {
// float pitch_fraction = -thrust_north / thrust_total;
// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
// if (roll_fraction < -1) {
// roll_fraction = -1;
// } else if (roll_fraction > 1) {
// roll_fraction = 1;
// }
// // if(counter % 5 == 0)
// // {
// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
// // fflush(stdout);
// // }
// pitch_desired = asinf(pitch_fraction);
// roll_desired = asinf(roll_fraction);
// }
// att_sp.roll = roll_desired;
// att_sp.pitch = pitch_desired;
// counter++;
// }

189
src/modules/multirotor_pos_control/thrust_pid.c Normal file
View File

@ -0,0 +1,189 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* 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 thrust_pid.c
*
* Implementation of thrust control PID.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "thrust_pid.h"
#include <math.h>
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->limit_min = limit_min;
pid->limit_max = limit_max;
pid->mode = mode;
pid->dt_min = dt_min;
pid->last_output = 0.0f;
pid->sp = 0.0f;
pid->error_previous = 0.0f;
pid->integral = 0.0f;
}
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
{
int ret = 0;
if (isfinite(kp)) {
pid->kp = kp;
} else {
ret = 1;
}
if (isfinite(ki)) {
pid->ki = ki;
} else {
ret = 1;
}
if (isfinite(kd)) {
pid->kd = kd;
} else {
ret = 1;
}
if (isfinite(limit_min)) {
pid->limit_min = limit_min;
} else {
ret = 1;
}
if (isfinite(limit_max)) {
pid->limit_max = limit_max;
} else {
ret = 1;
}
return ret;
}
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
{
/* Alternative integral component calculation
*
* start:
* error = setpoint - current_value
* integral = integral + (Ki * error * dt)
* derivative = (error - previous_error) / dt
* previous_error = error
* output = (Kp * error) + integral + (Kd * derivative)
* wait(dt)
* goto start
*/
if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
return pid->last_output;
}
float i, d;
pid->sp = sp;
// Calculated current error value
float error = pid->sp - val;
// Calculate or measured current error derivative
if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = error;
} else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = -val;
} else {
d = 0.0f;
}
if (!isfinite(d)) {
d = 0.0f;
}
/* calculate the error integral */
i = pid->integral + (pid->ki * error * dt);
/* attitude-thrust compensation
* r22 is (2, 2) componet of rotation matrix for current attitude */
float att_comp;
if (r22 > 0.8f)
att_comp = 1.0f / r22;
else if (r22 > 0.0f)
att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
else
att_comp = 1.0f;
/* calculate output */
float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
/* check for saturation */
if (output < pid->limit_min || output > pid->limit_max) {
/* saturated, recalculate output with old integral */
output = (error * pid->kp) + pid->integral + (d * pid->kd);
} else {
if (isfinite(i)) {
pid->integral = i;
}
}
if (isfinite(output)) {
if (output > pid->limit_max) {
output = pid->limit_max;
} else if (output < pid->limit_min) {
output = pid->limit_min;
}
pid->last_output = output;
}
return pid->last_output;
}
__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
{
pid->integral = i;
}

76
src/modules/multirotor_pos_control/thrust_pid.h Normal file
View File

@ -0,0 +1,76 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* 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 thrust_pid.h
*
* Definition of thrust control PID interface.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef THRUST_PID_H_
#define THRUST_PID_H_
#include <stdint.h>
__BEGIN_DECLS
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */
#define THRUST_PID_MODE_DERIVATIV_CALC 0
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */
#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1
typedef struct {
float kp;
float ki;
float kd;
float sp;
float integral;
float error_previous;
float last_output;
float limit_min;
float limit_max;
float dt_min;
uint8_t mode;
} thrust_pid_t;
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min);
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max);
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
__END_DECLS
#endif /* THRUST_PID_H_ */

31
src/modules/position_estimator_inav/inertial_filter.c Normal file
View File

@ -0,0 +1,31 @@
/*
* inertial_filter.c
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#include "inertial_filter.h"
void inertial_filter_predict(float dt, float x[3])
{
x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
x[1] += x[2] * dt;
}
void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
{
float ewdt = w * dt;
if (ewdt > 1.0f)
ewdt = 1.0f; // prevent over-correcting
ewdt *= e;
x[i] += ewdt;
if (i == 0) {
x[1] += w * ewdt;
x[2] += w * w * ewdt / 3.0;
} else if (i == 1) {
x[2] += w * ewdt;
}
}

13
src/modules/position_estimator_inav/inertial_filter.h Normal file
View File

@ -0,0 +1,13 @@
/*
* inertial_filter.h
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#include <stdbool.h>
#include <drivers/drv_hrt.h>
void inertial_filter_predict(float dt, float x[3]);
void inertial_filter_correct(float e, float dt, float x[3], int i, float w);

41
src/modules/position_estimator_inav/module.mk Normal file
View File

@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
#
# Makefile to build position_estimator_inav
#
MODULE_COMMAND = position_estimator_inav
SRCS = position_estimator_inav_main.c \
position_estimator_inav_params.c \
inertial_filter.c

589
src/modules/position_estimator_inav/position_estimator_inav_main.c Normal file
View File

@ -0,0 +1,589 @@
/****************************************************************************
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* 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 position_estimator_inav_main.c
* Model-identification based position estimator for multirotors
*/
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <string.h>
#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/optical_flow.h>
#include <mavlink/mavlink_log.h>
#include <poll.h>
#include <systemlib/err.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include <systemlib/conversions.h>
#include <drivers/drv_hrt.h>
#include "position_estimator_inav_params.h"
#include "inertial_filter.h"
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 hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
int position_estimator_inav_thread_main(int argc, char *argv[]);
static void usage(const char *reason);
/**
* Print the correct usage.
*/
static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr,
"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1);
}
/**
* The position_estimator_inav_thread 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 position_estimator_inav_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("position_estimator_inav already running\n");
/* this is not an error */
exit(0);
}
verbose_mode = false;
if (argc > 1)
if (!strcmp(argv[2], "-v"))
verbose_mode = true;
thread_should_exit = false;
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
position_estimator_inav_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("\tposition_estimator_inav is running\n");
} else {
printf("\tposition_estimator_inav not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/****************************************************************************
* main
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
warnx("started.");
int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[inav] started");
/* initialize values */
float x_est[3] = { 0.0f, 0.0f, 0.0f };
float y_est[3] = { 0.0f, 0.0f, 0.0f };
float z_est[3] = { 0.0f, 0.0f, 0.0f };
int baro_init_cnt = 0;
int baro_init_num = 200;
float baro_alt0 = 0.0f; /* to determine while start up */
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
/* declare and safely initialize all structs */
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status));
/* make sure that baroINITdone = false */
struct sensor_combined_s sensor;
memset(&sensor, 0, sizeof(sensor));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
/* subscribe */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
/* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
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);
/* first parameters read at start up */
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
/* first parameters update */
parameters_update(&pos_inav_param_handles, &params);
struct pollfd fds_init[1] = {
{ .fd = sensor_combined_sub, .events = POLLIN },
};
/* wait for initial baro value */
bool wait_baro = true;
thread_running = true;
while (wait_baro && !thread_should_exit) {
int ret = poll(fds_init, 1, 1000);
if (ret < 0) {
/* poll error */
errx(1, "subscriptions poll error on init.");
} else if (ret > 0) {
if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (wait_baro && sensor.baro_counter > baro_counter) {
baro_counter = sensor.baro_counter;
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
baro_alt0 += sensor.baro_alt_meter;
baro_init_cnt++;
} else {
wait_baro = false;
baro_alt0 /= (float) baro_init_cnt;
warnx("init baro: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
local_pos.z_valid = true;
local_pos.v_z_valid = true;
local_pos.global_z = true;
}
}
}
}
}
bool ref_xy_inited = false;
hrt_abstime ref_xy_init_start = 0;
const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix
hrt_abstime t_prev = 0;
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
uint32_t updates_counter_len = 1000000;
hrt_abstime pub_last = hrt_absolute_time();
uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D
float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float baro_corr = 0.0f; // D
float gps_corr[2][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
};
float sonar_corr = 0.0f;
float sonar_corr_filtered = 0.0f;
float flow_corr[] = { 0.0f, 0.0f }; // X, Y
float sonar_prev = 0.0f;
hrt_abstime sonar_time = 0;
/* main loop */
struct pollfd fds[6] = {
{ .fd = parameter_update_sub, .events = POLLIN },
{ .fd = vehicle_status_sub, .events = POLLIN },
{ .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = sensor_combined_sub, .events = POLLIN },
{ .fd = optical_flow_sub, .events = POLLIN },
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
};
if (!thread_should_exit) {
warnx("main loop started.");
}
while (!thread_should_exit) {
int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
/* poll error */
warnx("subscriptions poll error.");
thread_should_exit = true;
continue;
} else if (ret > 0) {
/* parameter update */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub,
&update);
/* update parameters */
parameters_update(&pos_inav_param_handles, &params);
}
/* vehicle status */
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub,
&vehicle_status);
}
/* vehicle attitude */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
}
/* sensor combined */
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter > accel_counter) {
if (att.R_valid) {
/* correct accel bias, now only for Z */
sensor.accelerometer_m_s2[2] -= accel_bias[2];
/* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) {
accel_NED[i] = 0.0f;
for (int j = 0; j < 3; j++) {
accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
}
}
accel_corr[0] = accel_NED[0] - x_est[2];
accel_corr[1] = accel_NED[1] - y_est[2];
accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
} else {
memset(accel_corr, 0, sizeof(accel_corr));
}
accel_counter = sensor.accelerometer_counter;
accel_updates++;
}
if (sensor.baro_counter > baro_counter) {
baro_corr = - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter;
baro_updates++;
}
}
/* optical flow */
if (fds[4].revents & POLLIN) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
if (flow.ground_distance_m != sonar_prev) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
sonar_corr = -flow.ground_distance_m - z_est[0];
sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
if (fabsf(sonar_corr) > params.sonar_err) {
// correction is too large: spike or new ground level?
if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
// spike detected, ignore
sonar_corr = 0.0f;
} else {
// new ground level
baro_alt0 += sonar_corr;
warnx("new home: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
z_est[0] += sonar_corr;
sonar_corr = 0.0f;
sonar_corr_filtered = 0.0f;
}
}
}
} else {
sonar_corr = 0.0f;
}
flow_updates++;
}
if (fds[5].revents & POLLIN) {
/* vehicle GPS position */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3) {
/* initialize reference position if needed */
if (ref_xy_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
/* calculate correction for position */
gps_corr[0][0] = gps_proj[0] - x_est[0];
gps_corr[1][0] = gps_proj[1] - y_est[0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
} else {
gps_corr[0][1] = 0.0f;
gps_corr[1][1] = 0.0f;
}
} else {
hrt_abstime t = hrt_absolute_time();
if (ref_xy_init_start == 0) {
ref_xy_init_start = t;
} else if (t > ref_xy_init_start + ref_xy_init_delay) {
ref_xy_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
local_pos.ref_lat = gps.lat;
local_pos.ref_lon = gps.lon;
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(lat, lon);
warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon);
mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
}
}
gps_updates++;
} else {
/* no GPS lock */
memset(gps_corr, 0, sizeof(gps_corr));
}
}
}
/* end of poll return value check */
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
t_prev = t;
/* accel bias correction, now only for Z
* not strictly correct, but stable and works */
accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est);
/* inertial filter correction for altitude */
baro_alt0 += sonar_corr * params.w_alt_sonar * dt;
inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro);
inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
bool flow_valid = false; // TODO implement opt flow
/* try to estimate xy even if no absolute position source available,
* if using optical flow velocity will be correct in this case */
bool can_estimate_xy = gps_valid || flow_valid;
if (can_estimate_xy) {
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est);
inertial_filter_predict(dt, y_est);
/* inertial filter correction for position */
inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
if (gps_valid) {
inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_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",
accel_updates / updates_dt,
baro_updates / updates_dt,
gps_updates / updates_dt,
attitude_updates / updates_dt,
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;
/* publish local position */
local_pos.timestamp = t;
local_pos.xy_valid = can_estimate_xy && gps_valid;
local_pos.v_xy_valid = can_estimate_xy;
local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented
local_pos.x = x_est[0];
local_pos.vx = x_est[1];
local_pos.y = y_est[0];
local_pos.vy = y_est[1];
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
local_pos.landed = false; // TODO
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
/* publish global position */
global_pos.valid = local_pos.global_xy;
if (local_pos.global_xy) {
double est_lat, est_lon;
map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.lat = (int32_t)(est_lat * 1e7);
global_pos.lon = (int32_t)(est_lon * 1e7);
global_pos.time_gps_usec = gps.time_gps_usec;
}
/* set valid values even if position is not valid */
if (local_pos.v_xy_valid) {
global_pos.vx = local_pos.vx;
global_pos.vy = local_pos.vy;
global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);
}
if (local_pos.z_valid) {
global_pos.relative_alt = -local_pos.z;
}
if (local_pos.global_z) {
global_pos.alt = local_pos.ref_alt - local_pos.z;
}
if (local_pos.v_z_valid) {
global_pos.vz = local_pos.vz;
}
global_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
}
warnx("exiting.");
mavlink_log_info(mavlink_fd, "[inav] exiting");
thread_running = false;
return 0;
}

87
src/modules/position_estimator_inav/position_estimator_inav_params.c Normal file
View File

@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* 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 position_estimator_inav_params.c
*
* Parameters for position_estimator_inav
*/
#include "position_estimator_inav_params.h"
PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
h->w_alt_baro = param_find("INAV_W_ALT_BARO");
h->w_alt_acc = param_find("INAV_W_ALT_ACC");
h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
h->w_pos_acc = param_find("INAV_W_POS_ACC");
h->w_pos_flow = param_find("INAV_W_POS_FLOW");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K");
h->sonar_filt = param_find("INAV_SONAR_FILT");
h->sonar_err = param_find("INAV_SONAR_ERR");
return OK;
}
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
{
param_get(h->w_alt_baro, &(p->w_alt_baro));
param_get(h->w_alt_acc, &(p->w_alt_acc));
param_get(h->w_alt_sonar, &(p->w_alt_sonar));
param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
param_get(h->w_pos_acc, &(p->w_pos_acc));
param_get(h->w_pos_flow, &(p->w_pos_flow));
param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k));
param_get(h->sonar_filt, &(p->sonar_filt));
param_get(h->sonar_err, &(p->sonar_err));
return OK;
}

81
src/modules/position_estimator_inav/position_estimator_inav_params.h Normal file
View File

@ -0,0 +1,81 @@
/****************************************************************************
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* 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 position_estimator_inav_params.h
*
* Parameters for Position Estimator
*/
#include <systemlib/param/param.h>
struct position_estimator_inav_params {
float w_alt_baro;
float w_alt_acc;
float w_alt_sonar;
float w_pos_gps_p;
float w_pos_gps_v;
float w_pos_acc;
float w_pos_flow;
float w_acc_bias;
float flow_k;
float sonar_filt;
float sonar_err;
};
struct position_estimator_inav_param_handles {
param_t w_alt_baro;
param_t w_alt_acc;
param_t w_alt_sonar;
param_t w_pos_gps_p;
param_t w_pos_gps_v;
param_t w_pos_acc;
param_t w_pos_flow;
param_t w_acc_bias;
param_t flow_k;
param_t sonar_filt;
param_t sonar_err;
};
/**
* Initialize all parameter handles and values
*
*/
int 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);

178
src/modules/sdlog2/sdlog2.c
View File

@ -60,7 +60,6 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@ -76,7 +75,7 @@
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_control_debug.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
@ -96,7 +95,6 @@
log_msgs_written++; \
} else { \
log_msgs_skipped++; \
/*printf("skip\n");*/ \
}
#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \
@ -104,9 +102,6 @@
fds[fdsc_count].events = POLLIN; \
fdsc_count++;
//#define SDLOG2_DEBUG
static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
@ -194,7 +189,7 @@ static int file_copy(const char *file_old, const char *file_new);
static void handle_command(struct vehicle_command_s *cmd);
static void handle_status(struct actuator_armed_s *armed);
static void handle_status(struct vehicle_status_s *cmd);
/**
* Create folder for current logging session. Store folder name in 'log_folder'.
@ -235,7 +230,7 @@ int sdlog2_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("sdlog2 already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
@ -252,7 +247,7 @@ int sdlog2_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (!thread_running) {
printf("\tsdlog2 is not started\n");
warnx("not started");
}
main_thread_should_exit = true;
@ -264,7 +259,7 @@ int sdlog2_main(int argc, char *argv[])
sdlog2_status();
} else {
printf("\tsdlog2 not started\n");
warnx("not started\n");
}
exit(0);
@ -389,11 +384,6 @@ static void *logwriter_thread(void *arg)
/* only get pointer to thread-safe data, do heavy I/O a few lines down */
int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part);
#ifdef SDLOG2_DEBUG
int rp = logbuf->read_ptr;
int wp = logbuf->write_ptr;
#endif
/* continue */
pthread_mutex_unlock(&logbuffer_mutex);
@ -409,9 +399,6 @@ static void *logwriter_thread(void *arg)
n = write(log_file, read_ptr, n);
should_wait = (n == available) && !is_part;
#ifdef SDLOG2_DEBUG
printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait);
#endif
if (n < 0) {
main_thread_should_exit = true;
@ -424,14 +411,8 @@ static void *logwriter_thread(void *arg)
} else {
n = 0;
#ifdef SDLOG2_DEBUG
printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit);
#endif
/* exit only with empty buffer */
if (main_thread_should_exit || logwriter_should_exit) {
#ifdef SDLOG2_DEBUG
printf("break logwriter thread\n");
#endif
break;
}
should_wait = true;
@ -446,10 +427,6 @@ static void *logwriter_thread(void *arg)
fsync(log_file);
close(log_file);
#ifdef SDLOG2_DEBUG
printf("logwriter thread exit\n");
#endif
return OK;
}
@ -606,15 +583,6 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "unable to create logging folder, exiting.");
}
const char *converter_in = "/etc/logging/conv.zip";
char* converter_out = malloc(150);
sprintf(converter_out, "%s/conv.zip", folder_path);
if (file_copy(converter_in, converter_out)) {
errx(1, "unable to copy conversion scripts, exiting.");
}
free(converter_out);
/* only print logging path, important to find log file later */
warnx("logging to directory: %s", folder_path);
@ -625,21 +593,9 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "can't allocate log buffer, exiting.");
}
/* --- IMPORTANT: DEFINE MAX NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* max number of messages */
const ssize_t fdsc = 21;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
struct vehicle_status_s buf_status;
memset(&buf_status, 0, sizeof(buf_status));
struct actuator_armed_s buf_armed;
memset(&buf_armed, 0, sizeof(buf_armed));
/* warning! using union here to save memory, elements should be used separately! */
union {
struct vehicle_command_s cmd;
@ -656,19 +612,18 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct vehicle_control_debug_s control_debug;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
} buf;
memset(&buf, 0, sizeof(buf));
struct {
int cmd_sub;
int status_sub;
int armed_sub;
int sensor_sub;
int att_sub;
int att_sp_sub;
@ -682,11 +637,11 @@ int sdlog2_thread_main(int argc, char *argv[])
int global_pos_sp_sub;
int gps_pos_sub;
int vicon_pos_sub;
int control_debug_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
} subs;
/* log message buffer: header + body */
@ -704,7 +659,6 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPS_s log_GPS;
struct log_ATTC_s log_ATTC;
struct log_STAT_s log_STAT;
struct log_CTRL_s log_CTRL;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
struct log_AIRS_s log_AIRS;
@ -713,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPOS_s log_GPOS;
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -720,18 +675,20 @@ int sdlog2_thread_main(int argc, char *argv[])
#pragma pack(pop)
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 20;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
/* --- VEHICLE COMMAND --- */
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
fds[fdsc_count].fd = subs.cmd_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR ARMED --- */
subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
fds[fdsc_count].fd = subs.armed_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VEHICLE STATUS --- */
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
fds[fdsc_count].fd = subs.status_sub;
@ -816,12 +773,6 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- CONTROL DEBUG --- */
subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug));
fds[fdsc_count].fd = subs.control_debug_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- OPTICAL FLOW --- */
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
fds[fdsc_count].fd = subs.flow_sub;
@ -846,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL VELOCITY SETPOINT --- */
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
fds[fdsc_count].fd = subs.global_vel_sp_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.
@ -905,33 +862,22 @@ int sdlog2_thread_main(int argc, char *argv[])
handled_topics++;
}
/* --- ARMED- LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed);
if (log_when_armed) {
handle_status(&buf_armed);
}
handled_topics++;
}
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
//if (log_when_armed) {
// handle_status(&buf_armed);
//}
if (log_when_armed) {
handle_status(&buf_status);
}
//handled_topics++;
handled_topics++;
}
if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
continue;
}
ifds = 2; // Begin from fds[2] again
ifds = 1; // Begin from fds[1] again
pthread_mutex_lock(&logbuffer_mutex);
@ -944,16 +890,9 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) {
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_STAT_MSG;
// XXX fix this
// log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
// log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
// log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
// log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
log_msg.body.log_STAT.state = 0;
log_msg.body.log_STAT.flight_mode = 0;
log_msg.body.log_STAT.manual_control_mode = 0;
log_msg.body.log_STAT.manual_sas_mode = 0;
log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */
log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state;
log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
@ -1044,9 +983,6 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
log_msg.body.log_ATT.roll_acc = buf.att.rollacc;
log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc;
log_msg.body.log_ATT.yaw_acc = buf.att.yawacc;
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
@ -1106,10 +1042,9 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
log_msg.body.log_LPOS.hdg = buf.local_pos.hdg;
log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat;
log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon;
log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt;
log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
LOGBUFFER_WRITE_AND_COUNT(LPOS);
}
@ -1162,27 +1097,6 @@ int sdlog2_thread_main(int argc, char *argv[])
// TODO not implemented yet
}
/* --- CONTROL DEBUG --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug);
log_msg.msg_type = LOG_CTRL_MSG;
log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p;
log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i;
log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d;
log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p;
log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i;
log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d;
log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p;
log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i;
log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d;
log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p;
log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i;
log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d;
LOGBUFFER_WRITE_AND_COUNT(CTRL);
}
/* --- FLOW --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
@ -1237,14 +1151,18 @@ int sdlog2_thread_main(int argc, char *argv[])
}
}
#ifdef SDLOG2_DEBUG
printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
#endif
/* --- GLOBAL VELOCITY SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp);
log_msg.msg_type = LOG_GVSP_MSG;
log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
#ifdef SDLOG2_DEBUG
printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
#endif
/* only request write if several packets can be written at once */
pthread_cond_signal(&logbuffer_cond);
}
@ -1327,7 +1245,7 @@ int file_copy(const char *file_old, const char *file_new)
fclose(source);
fclose(target);
return OK;
return ret;
}
void handle_command(struct vehicle_command_s *cmd)
@ -1357,10 +1275,12 @@ void handle_command(struct vehicle_command_s *cmd)
}
}
void handle_status(struct actuator_armed_s *armed)
void handle_status(struct vehicle_status_s *status)
{
if (armed->armed != flag_system_armed) {
flag_system_armed = armed->armed;
// TODO use flag from actuator_armed here?
bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
if (armed != flag_system_armed) {
flag_system_armed = armed;
if (flag_system_armed) {
sdlog2_start_log();

63
src/modules/sdlog2/sdlog2_messages.h
View File

@ -63,9 +63,6 @@ struct log_ATT_s {
float roll_rate;
float pitch_rate;
float yaw_rate;
float roll_acc;
float pitch_acc;
float yaw_acc;
};
/* --- ATSP - ATTITUDE SET POINT --- */
@ -109,10 +106,9 @@ struct log_LPOS_s {
float vx;
float vy;
float vz;
float hdg;
int32_t home_lat;
int32_t home_lon;
float home_alt;
int32_t ref_lat;
int32_t ref_lon;
float ref_alt;
};
/* --- LPSP - LOCAL POSITION SETPOINT --- */
@ -152,55 +148,36 @@ struct log_ATTC_s {
/* --- STAT - VEHICLE STATE --- */
#define LOG_STAT_MSG 10
struct log_STAT_s {
uint8_t state;
uint8_t flight_mode;
uint8_t manual_control_mode;
uint8_t manual_sas_mode;
uint8_t armed;
uint8_t main_state;
uint8_t navigation_state;
uint8_t arming_state;
float battery_voltage;
float battery_current;
float battery_remaining;
uint8_t battery_warning;
};
/* --- CTRL - CONTROL DEBUG --- */
#define LOG_CTRL_MSG 11
struct log_CTRL_s {
float roll_p;
float roll_i;
float roll_d;
float roll_rate_p;
float roll_rate_i;
float roll_rate_d;
float pitch_p;
float pitch_i;
float pitch_d;
float pitch_rate_p;
float pitch_rate_i;
float pitch_rate_d;
};
/* --- RC - RC INPUT CHANNELS --- */
#define LOG_RC_MSG 12
#define LOG_RC_MSG 11
struct log_RC_s {
float channel[8];
};
/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
#define LOG_OUT0_MSG 13
#define LOG_OUT0_MSG 12
struct log_OUT0_s {
float output[8];
};
/* --- AIRS - AIRSPEED --- */
#define LOG_AIRS_MSG 14
#define LOG_AIRS_MSG 13
struct log_AIRS_s {
float indicated_airspeed;
float true_airspeed;
};
/* --- ARSP - ATTITUDE RATE SET POINT --- */
#define LOG_ARSP_MSG 15
#define LOG_ARSP_MSG 14
struct log_ARSP_s {
float roll_rate_sp;
float pitch_rate_sp;
@ -208,7 +185,7 @@ struct log_ARSP_s {
};
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 16
#define LOG_FLOW_MSG 15
struct log_FLOW_s {
int16_t flow_raw_x;
int16_t flow_raw_y;
@ -264,22 +241,29 @@ struct log_ESC_s {
uint16_t esc_setpoint_raw;
};
/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
#define LOG_GVSP_MSG 19
struct log_GVSP_s {
float vx;
float vy;
float vz;
};
#pragma pack(pop)
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"),
LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
LOG_FORMAT(CTRL, "ffffffffffff", "RP,RI,RD,RRP,RRI,RRD,PP,PI,PD,PRP,PRI,PRD"),
LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,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"),
@ -287,7 +271,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);

22
src/modules/sensors/sensors.cpp
View File

@ -284,6 +284,7 @@ private:
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
struct {
float min[_rc_max_chan_count];
@ -535,7 +536,8 @@ Sensors::Sensors() :
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_board_rotation(3,3),
_external_mag_rotation(3,3)
_external_mag_rotation(3,3),
_mag_is_external(false)
{
/* basic r/c parameters */
@ -941,6 +943,14 @@ Sensors::mag_init()
/* set the driver to poll at 150Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 150);
int ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
if (ret < 0)
errx(1, "FATAL: unknown if magnetometer is external or onboard");
else if (ret == 1)
_mag_is_external = true;
else
_mag_is_external = false;
close(fd);
}
@ -953,7 +963,7 @@ Sensors::baro_init()
if (fd < 0) {
warn("%s", BARO_DEVICE_PATH);
warnx("No barometer found, ignoring");
errx(1, "FATAL: No barometer found");
}
/* set the driver to poll at 150Hz */
@ -1037,10 +1047,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
// XXX TODO add support for external mag orientation
math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
vect = _board_rotation*vect;
if (_mag_is_external)
vect = _external_mag_rotation*vect;
else
vect = _board_rotation*vect;
raw.magnetometer_ga[0] = vect(0);
raw.magnetometer_ga[1] = vect(1);

6
src/modules/systemlib/pid/pid.c
View File

@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
* @param dt
* @return
*/
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d)
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
{
/* error = setpoint - actual_position
integral = integral + (error*dt)
@ -196,10 +196,6 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
pid->last_output = output;
}
*ctrl_p = (error * pid->kp);
*ctrl_i = (i * pid->ki);
*ctrl_d = (d * pid->kd);
return pid->last_output;
}

2
src/modules/systemlib/pid/pid.h
View File

@ -85,7 +85,7 @@ typedef struct {
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
//void pid_set(PID_t *pid, float sp);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
__EXPORT void pid_reset_integral(PID_t *pid);

3
src/modules/uORB/objects_common.cpp
View File

@ -120,6 +120,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
#include "topics/vehicle_global_position_set_triplet.h"
ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
#include "topics/vehicle_global_velocity_setpoint.h"
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
#include "topics/mission.h"
ORB_DEFINE(mission, struct mission_s);

1
src/modules/uORB/topics/actuator_controls.h
View File

@ -55,7 +55,6 @@
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
/**
* @addtogroup topics
* @{

5
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -64,6 +64,8 @@ struct vehicle_control_mode_s
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
bool flag_armed;
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
// XXX needs yet to be set by state machine helper
@ -75,9 +77,10 @@ struct vehicle_control_mode_s
//bool flag_auto_enabled;
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */

40
src/modules/multirotor_pos_control/position_control.h → src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
View File

@ -1,10 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,15 +33,32 @@
****************************************************************************/
/**
* @file multirotor_position_control.h
* Definition of the position control for a multirotor VTOL
* @file vehicle_global_velocity_setpoint.h
* Definition of the global velocity setpoint uORB topic.
*/
// #ifndef POSITION_CONTROL_H_
// #define POSITION_CONTROL_H_
#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
#include "../uORB.h"
// #endif /* POSITION_CONTROL_H_ */
/**
* @addtogroup topics
* @{
*/
struct vehicle_global_velocity_setpoint_s
{
float vx; /**< in m/s NED */
float vy; /**< in m/s NED */
float vz; /**< in m/s NED */
}; /**< Velocity setpoint in NED frame */
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_global_velocity_setpoint);
#endif

40
src/modules/uORB/topics/vehicle_local_position.h
View File

@ -54,27 +54,27 @@
*/
struct vehicle_local_position_s
{
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
bool valid; /**< true if position satisfies validity criteria of estimator */
float x; /**< X positin in meters in NED earth-fixed frame */
float y; /**< X positin in meters in NED earth-fixed frame */
float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */
float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */
float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */
float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */
float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */
float hdg; /**< Compass heading in radians -PI..+PI. */
// TODO Add covariances here
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
bool xy_valid; /**< true if x and y are valid */
bool z_valid; /**< true if z is valid */
bool v_xy_valid; /**< true if vy and vy are valid */
bool v_z_valid; /**< true if vz is valid */
/* Position in local NED frame */
float x; /**< X position in meters in NED earth-fixed frame */
float y; /**< X position in meters in NED earth-fixed frame */
float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */
/* Velocity in NED frame */
float vx; /**< Ground X Speed (Latitude), m/s in NED */
float vy; /**< Ground Y Speed (Longitude), m/s in NED */
float vz; /**< Ground Z Speed (Altitude), m/s in NED */
/* Reference position in GPS / WGS84 frame */
uint64_t home_timestamp;/**< Time when home position was set */
int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */
int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */
float home_alt; /**< Altitude in meters LOGME */
float home_hdg; /**< Compass heading in radians -PI..+PI. */
bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */
uint64_t ref_timestamp; /**< Time when reference position was set */
int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */
int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */
float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
bool landed; /**< true if vehicle is landed */
};
/**

4
src/modules/uORB/topics/vehicle_status.h
View File

@ -68,8 +68,7 @@ typedef enum {
/* navigation state machine */
typedef enum {
NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed
NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization
NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization
NAVIGATION_STATE_STABILIZE, // attitude stabilization
NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization
NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization
@ -203,6 +202,7 @@ struct vehicle_status_s
bool condition_launch_position_valid; /**< indicates a valid launch position */
bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool condition_local_position_valid;
bool condition_local_altitude_valid;
bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
bool condition_landed; /**< true if vehicle is landed, always true if disarmed */

31
src/systemcmds/nshterm/nshterm.c
View File

@ -40,6 +40,7 @@
*/
#include <nuttx/config.h>
#include <termios.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdarg.h>
@ -48,6 +49,7 @@
#include <errno.h>
#include <apps/nsh.h>
#include <fcntl.h>
#include <systemlib/err.h>
__EXPORT int nshterm_main(int argc, char *argv[]);
@ -61,8 +63,8 @@ nshterm_main(int argc, char *argv[])
uint8_t retries = 0;
int fd = -1;
while (retries < 5) {
// the retries are to cope with the behaviour of /dev/ttyACM0
// which may not be ready immediately.
/* the retries are to cope with the behaviour of /dev/ttyACM0 */
/* which may not be ready immediately. */
fd = open(argv[1], O_RDWR);
if (fd != -1) {
break;
@ -74,7 +76,30 @@ nshterm_main(int argc, char *argv[])
perror(argv[1]);
exit(1);
}
// setup standard file descriptors
/* set up the serial port with output processing */
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
warnx("ERROR get termios config %s: %d\n", argv[1], termios_state);
close(fd);
return -1;
}
/* Set ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/);
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]);
close(fd);
return -1;
}
/* setup standard file descriptors */
close(0);
close(1);
close(2);

2
src/systemcmds/preflight_check/preflight_check.c
View File

@ -263,7 +263,7 @@ system_eval:
led_toggle(leds, LED_BLUE);
/* display and sound error */
for (int i = 0; i < 150; i++)
for (int i = 0; i < 50; i++)
{
led_toggle(leds, LED_BLUE);
led_toggle(leds, LED_AMBER);

18
src/systemcmds/top/top.c
View File

@ -107,9 +107,6 @@ top_main(void)
float interval_time_ms_inv = 0.f;
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
/* clear screen */
printf("\033[2J");
@ -256,17 +253,24 @@ top_main(void)
interval_start_time = new_time;
/* Sleep 200 ms waiting for user input five times ~ 1s */
/* XXX use poll ... */
for (int k = 0; k < 5; k++) {
char c;
if (read(console, &c, 1) == 1) {
struct pollfd fds;
int ret;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'c':
case 'q':
close(console);
return OK;
/* not reached */
}
@ -278,7 +282,5 @@ top_main(void)
new_time = hrt_absolute_time();
}
close(console);
return OK;
}