Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

manual merge of origin/master into fw_control

Browse Source
This commit is contained in:
Thomas Gubler
2012-11-21 21:35:13 +01:00
parent 1f798efd17 e24c349d1d
commit 6fb54ec62c
59 changed files with 2088 additions and 355 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/Makefile
+4
View File
@@ -29,6 +29,10 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \
$(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \
$(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \
$(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \
$(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \
$(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \
$(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \
$(SRCROOT)/mixers/FMU_octo_+.mix~mixers/FMU_octo_+.mix \
$(SRCROOT)/logging/logconv.m~logging/logconv.m
#
ROMFS/logging/logconv.m
+10 -1
View File
@@ -32,7 +32,7 @@ if exist(filePath, 'file')
fileSize = fileInfo.bytes;
fid = fopen(filePath, 'r');
elements = int64(fileSize./(16*4+8))/4
elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+1+3+3+3+3+9+6+4+6)*4))
for i=1:elements
% timestamp
@@ -79,6 +79,15 @@ if exist(filePath, 'file')
% RotMatrix (9 channels)
sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le');
% vicon position (6 channels)
sensors(i,48:53) = fread(fid, 6, 'float', 0, 'ieee-le');
% actuator control effective (4 channels)
sensors(i,54:57) = fread(fid, 4, 'float', 0, 'ieee-le');
% optical flow (6 values)
sensors(i,58:63) = fread(fid, 6, 'float', 0, 'ieee-le');
end
time_us = sensors(elements,1) - sensors(1,1);
time_s = time_us*1e-6
ROMFS/mixers/FMU_hex_+.mix
+7
View File
@@ -0,0 +1,7 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a hexacopter in the + configuration. All controls
are mixed 100%.
R: 6+ 10000 10000 10000 0
ROMFS/mixers/FMU_hex_x.mix
+7
View File
@@ -0,0 +1,7 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a hexacopter in the X configuration. All controls
are mixed 100%.
R: 6x 10000 10000 10000 0
ROMFS/mixers/FMU_octo_+.mix
+7
View File
@@ -0,0 +1,7 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a octocopter in the + configuration. All controls
are mixed 100%.
R: 8+ 10000 10000 10000 0
ROMFS/mixers/FMU_octo_x.mix
+7
View File
@@ -0,0 +1,7 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a octocopter in the X configuration. All controls
are mixed 100%.
R: 8x 10000 10000 10000 0
ROMFS/scripts/rc.sensors
+15 -10
View File
@@ -8,21 +8,26 @@
#
ms5611 start
mpu6000 start
hmc5883 start
if mpu6000 start
then
echo "using MPU6000 and HMC5883L"
hmc5883 start
else
echo "using L3GD20 and LSM303D"
l3gd20 start
lsm303 start
fi
#
# Start the sensor collection task.
# IMPORTANT: this also loads param offsets
# ALWAYS start this task before the
# preflight_check.
#
sensors start
#
# Test sensor functionality
# Check sensors - run AFTER 'sensors start'
#
# XXX integrate with 'sensors start' ?
#
#if sensors quicktest
#then
# echo "[init] sensor initialisation FAILED."
# reboot
#fi
preflight_check
ROMFS/scripts/rc.standalone
-64
View File
@@ -10,68 +10,4 @@ echo "[init] doing standalone PX4FMU startup..."
#
uorb start
#
# Init the EEPROM
#
echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
then
param load
fi
#
# Start the sensors.
#
#sh /etc/init.d/rc.sensors
#
# Start MAVLink
#
# mavlink -d /dev/ttyS0 -b 57600 &
#
# Start the commander.
#
# XXX this should be 'commander start'.
#
#commander &
#
# Start the attitude estimator
#
# XXX this should be '<command> start'.
#
#attitude_estimator_bm &
#position_estimator &
#
# Start the fixed-wing controller.
#
# XXX should this be looking for configuration to decide
# whether the board is configured for fixed-wing use?
#
# XXX this should be 'fixedwing_control start'.
#
#fixedwing_control &
#
# Configure FMU for standalone mode
#
# XXX arguments?
#
#px4fmu start
#
# Start looking for a GPS.
#
# XXX this should not need to be backgrounded
#
#gps -d /dev/ttyS3 -m all &
#
# Start logging to microSD if we can
#
sh /etc/init.d/rc.logging
echo "[init] startup done"
apps/ardrone_interface/ardrone_motor_control.c
+28 -4
View File
@@ -45,6 +45,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <systemlib/err.h>
#include "ardrone_motor_control.h"
@@ -385,6 +386,11 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
float yaw_factor = 1.0f;
static bool initialized = false;
/* publish effective outputs */
static struct actuator_controls_effective_s actuator_controls_effective;
static orb_advert_t actuator_controls_effective_pub;
if (motor_thrust <= min_thrust) {
motor_thrust = min_thrust;
output_band = 0.0f;
@@ -417,17 +423,18 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
&& motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
yaw_factor = 0.5f;
yaw_control *= yaw_factor;
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor);
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor);
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor);
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor);
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
}
for (int i = 0; i < 4; i++) {
@@ -441,6 +448,23 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
}
}
/* publish effective outputs */
actuator_controls_effective.control_effective[0] = roll_control;
actuator_controls_effective.control_effective[1] = pitch_control;
/* yaw output after limiting */
actuator_controls_effective.control_effective[2] = yaw_control;
/* possible motor thrust limiting */
actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
if (!initialized) {
/* advertise and publish */
actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
initialized = true;
} else {
/* already initialized, just publishing */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
}
/* set the motor values */
/* scale up from 0..1 to 10..512) */
apps/commander/commander.c
+5 -4
View File
@@ -63,7 +63,6 @@
#include <string.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include "state_machine_helper.h"
#include "systemlib/systemlib.h"
@@ -269,6 +268,7 @@ void tune_confirm(void) {
void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
{
/* set to mag calibration mode */
status->flag_preflight_mag_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
@@ -325,7 +325,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
uint64_t axis_deadline = hrt_absolute_time();
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
const char axislabels[3] = { 'X', 'Z', 'Y'};
const char axislabels[3] = { 'X', 'Y', 'Z'};
int axis_index = -1;
float *x = (float*)malloc(sizeof(float) * calibration_maxcount);
@@ -471,6 +471,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
int save_ret = param_save_default();
if(save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "[cmd] FAILED storing calibration");
}
printf("[mag cal]\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
@@ -1132,7 +1133,7 @@ int commander_main(int argc, char *argv[])
daemon_task = task_spawn("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 50,
4096,
4000,
commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
@@ -1392,7 +1393,7 @@ int commander_thread_main(int argc, char *argv[])
else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CRITICAL BATTERY!");
state_machine_emergency(stat_pub, &current_status, mavlink_fd);
}
apps/commander/state_machine_helper.c
+5 -6
View File
@@ -238,11 +238,8 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
void publish_armed_status(const struct vehicle_status_s *current_status) {
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
/* lock down actuators if required */
// XXX FIXME Currently any loss of RC will completely disable all actuators
// needs proper failsafe
armed.lockdown = ((current_status->rc_signal_lost && current_status->offboard_control_signal_lost)
|| current_status->flag_hil_enabled) ? true : false;
/* lock down actuators if required, only in HIL */
armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@@ -260,7 +257,9 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
// DO NOT abort mission
//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
} else {
fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
apps/drivers/drv_accel.h
+3
View File
@@ -115,4 +115,7 @@ ORB_DECLARE(sensor_accel);
/** get the current accel measurement range in g */
#define ACCELIOCGRANGE _ACCELIOC(8)
/** get the result of a sensor self-test */
#define ACCELIOCSELFTEST _ACCELIOC(9)
#endif /* _DRV_ACCEL_H */
apps/drivers/drv_gyro.h
+3
View File
@@ -111,4 +111,7 @@ ORB_DECLARE(sensor_gyro);
/** get the current gyro measurement range in degrees per second */
#define GYROIOCGRANGE _GYROIOC(7)
/** check the status of the sensor */
#define GYROIOCSELFTEST _GYROIOC(8)
#endif /* _DRV_GYRO_H */
apps/drivers/drv_mag.h
+3
View File
@@ -108,4 +108,7 @@ ORB_DECLARE(sensor_mag);
/** excite strap */
#define MAGIOCEXSTRAP _MAGIOC(6)
/** perform self test and report status */
#define MAGIOCSELFTEST _MAGIOC(7)
#endif /* _DRV_MAG_H */
apps/drivers/hmc5883/hmc5883.cpp
+20 -13
View File
@@ -634,7 +634,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
return check_calibration();
/* check calibration, but not actually return an error */
(void)check_calibration();
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
@@ -647,6 +649,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCEXSTRAP:
return set_excitement(arg);
case MAGIOCSELFTEST:
return check_calibration();
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@@ -1029,27 +1034,27 @@ int HMC5883::check_calibration()
{
bool scale_valid, offset_valid;
if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) &&
(-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) &&
(-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) {
/* scale is different from one */
scale_valid = true;
} else {
if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) &&
(-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
(-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) {
/* scale is one */
scale_valid = false;
} else {
scale_valid = true;
}
if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
/* offset is different from zero */
offset_valid = true;
} else {
/* offset is zero */
offset_valid = false;
} else {
offset_valid = true;
}
if (_calibrated != (offset_valid && scale_valid)) {
warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ",
(offset_valid) ? "" : "offset invalid.");
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid);
/* notify about state change */
struct subsystem_info_s info = {
@@ -1059,7 +1064,9 @@ int HMC5883::check_calibration()
SUBSYSTEM_TYPE_MAG};
orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
return 0;
/* return 0 if calibrated, 1 else */
return (!_calibrated);
}
int HMC5883::set_excitement(unsigned enable)
apps/drivers/mpu6000/mpu6000.cpp
+40 -4
View File
@@ -260,6 +260,13 @@ private:
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
* Self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
};
/**
@@ -494,6 +501,17 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
return ret;
}
int
MPU6000::self_test()
{
if (_reads == 0) {
measure();
}
/* return 0 on success, 1 else */
return (_reads > 0) ? 0 : 1;
}
ssize_t
MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
{
@@ -592,9 +610,17 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return -EINVAL;
case ACCELIOCSSCALE:
/* copy scale in */
memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
return OK;
{
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
memcpy(&_accel_scale, s, sizeof(_accel_scale));
return OK;
} else {
return -EINVAL;
}
}
case ACCELIOCGSCALE:
/* copy scale out */
@@ -609,6 +635,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// _accel_range_rad_s = 8.0f * 9.81f;
return -EINVAL;
case ACCELIOCSELFTEST:
return self_test();
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
@@ -656,6 +685,9 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
// _gyro_range_m_s2 = xx
return -EINVAL;
case GYROIOCSELFTEST:
return self_test();
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
@@ -813,7 +845,11 @@ MPU6000::measure()
* Fetch the full set of measurements from the MPU6000 in one pass.
*/
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
return;
/* count measurement */
_reads++;
/*
* Convert from big to little endian
apps/drivers/px4io/px4io.cpp
+2 -3
View File
@@ -37,7 +37,6 @@
*
* PX4IO is connected via serial (or possibly some other interface at a later
* point).
*/
#include <nuttx/config.h>
@@ -384,7 +383,7 @@ PX4IO::task_main()
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
_send_needed = true;
}
}
@@ -539,7 +538,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_DISARM:
/* fake a disarmed transition */
_armed.armed = true;
_armed.armed = false;
_send_needed = true;
break;
apps/fixedwing_att_control/fixedwing_att_control_att.c
+2 -2
View File
@@ -144,10 +144,10 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
}
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_tait_bryan, att->roll, 0, 0);
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
/* Pitch (P) */
float pitch_sp_rollcompensation = att_sp->pitch_tait_bryan + p.pitch_roll_compensation_p * att_sp->roll_tait_bryan;
float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body;
rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */
apps/fixedwing_pos_control/fixedwing_pos_control.h
-19
View File
@@ -42,26 +42,7 @@
#ifndef FIXEDWING_POS_CONTROL_H_
#define FIXEDWING_POS_CONTROL_H_
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#endif
struct planned_path_segments_s {
bool segment_type;
double start_lat; // Start of line or center of arc
double start_lon;
double end_lat;
double end_lon;
float radius; // Radius of arc
float arc_start_bearing; // Bearing from center to start of arc
float arc_sweep; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
};
float _wrap180(float bearing);
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+20 -69
View File
@@ -95,6 +95,19 @@ struct fw_pos_control_param_handles {
};
struct planned_path_segments_s {
bool segment_type;
double start_lat; // Start of line or center of arc
double start_lon;
double end_lat;
double end_lon;
float radius; // Radius of arc
float arc_start_bearing; // Bearing from center to start of arc
float arc_sweep; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
};
/* Prototypes */
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
@@ -185,9 +198,9 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
/* publish attitude setpoint */
attitude_setpoint.roll_tait_bryan = 0.0f;
attitude_setpoint.pitch_tait_bryan = 0.0f;
attitude_setpoint.yaw_tait_bryan = 0.0f;
attitude_setpoint.roll_body = 0.0f;
attitude_setpoint.pitch_body = 0.0f;
attitude_setpoint.yaw_body = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
/* subscribe */
@@ -261,7 +274,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("psi_track: %0.4f\n", psi_track);
printf("psi_track: %0.4f\n", (double)psi_track);
}
/* Control */
@@ -287,7 +300,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
float psi_e = psi_c - att.yaw;
/* shift error to prevent wrapping issues */
psi_e = _wrapPI(psi_e);
psi_e = _wrap_pi(psi_e);
/* calculate roll setpoint, do this artificially around zero */
//TODO: psi rate loop incomplete
@@ -301,8 +314,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
float psi_rate_e_scaled = psi_rate_e * sqrtf(pow(global_pos.vx,2) + pow(global_pos.vy,2)) / 9.81f; //* V_gr / g
attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
// if (counter % 100 == 0)
// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
@@ -319,7 +331,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
{
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_tait_bryan = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
/*Publish the attitude setpoint */
@@ -401,64 +413,3 @@ int fixedwing_pos_control_main(int argc, char *argv[])
usage("unrecognized command");
exit(1);
}
float _wrapPI(float bearing)
{
while (bearing > M_PI_F) {
bearing = bearing - M_TWOPI_F;
}
while (bearing <= -M_PI_F) {
bearing = bearing + M_TWOPI_F;
}
return bearing;
}
float _wrap2PI(float bearing)
{
while (bearing >= M_TWOPI_F) {
bearing = bearing - M_TWOPI_F;
}
while (bearing < 0.0f) {
bearing = bearing + M_TWOPI_F;
}
return bearing;
}
float _wrap180(float bearing)
{
while (bearing > 180.0f) {
bearing = bearing - 360.0f;
}
while (bearing <= -180.0f) {
bearing = bearing + 360.0f;
}
return bearing;
}
float _wrap360(float bearing)
{
while (bearing >= 360.0f) {
bearing = bearing - 360.0f;
}
while (bearing < 0.0f) {
bearing = bearing + 360.0f;
}
return bearing;
}
apps/gps/ubx.c
+1 -1
View File
@@ -49,7 +49,7 @@
#include <mavlink/mavlink_log.h>
#define UBX_HEALTH_SUCCESS_COUNTER_LIMIT 2
#define UBX_HEALTH_FAIL_COUNTER_LIMIT 2
#define UBX_HEALTH_FAIL_COUNTER_LIMIT 3
#define UBX_HEALTH_PROBE_COUNTER_LIMIT 4
#define UBX_BUFFER_SIZE 1000
apps/mavlink/mavlink.c
+3
View File
@@ -547,6 +547,9 @@ int mavlink_thread_main(int argc, char *argv[])
/* print welcome text */
warnx("MAVLink v1.0 serial interface starting...");
/* inform about mode */
warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
/* Flush stdout in case MAVLink is about to take it over */
fflush(stdout);
apps/mavlink/mavlink_receiver.c
+9
View File
@@ -206,6 +206,10 @@ handle_message(mavlink_message_t *msg)
vicon_position.y = pos.y;
vicon_position.z = pos.z;
vicon_position.roll = pos.roll;
vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw;
if (vicon_position_pub <= 0) {
vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
@@ -356,6 +360,11 @@ handle_message(mavlink_message_t *msg)
struct manual_control_setpoint_s mc;
static orb_advert_t mc_pub = 0;
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* get a copy first, to prevent altering values that are not sent by the mavlink command */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
mc.timestamp = rc_hil.timestamp;
mc.roll = man.x / 1000.0f;
mc.pitch = man.y / 1000.0f;
apps/mavlink/orb_listener.c
+17
View File
@@ -113,6 +113,7 @@ static void l_actuator_armed(struct listener *l);
static void l_manual_control_setpoint(struct listener *l);
static void l_vehicle_attitude_controls(struct listener *l);
static void l_debug_key_value(struct listener *l);
static void l_optical_flow(struct listener *l);
struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@@ -134,6 +135,7 @@ struct listener listeners[] = {
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
{l_optical_flow, &mavlink_subs.optical_flow, 0},
};
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -586,6 +588,17 @@ l_debug_key_value(struct listener *l)
debug.value);
}
void
l_optical_flow(struct listener *l)
{
struct optical_flow_s flow;
orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
}
static void *
uorb_receive_thread(void *arg)
{
@@ -710,6 +723,10 @@ uorb_receive_start(void)
mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */
/* --- FLOW SENSOR --- */
mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
/* start the listener loop */
pthread_attr_t uorb_attr;
pthread_attr_init(&uorb_attr);
apps/mavlink/orb_topics.h
+1
View File
@@ -77,6 +77,7 @@ struct mavlink_subscriptions {
int spg_sub;
int debug_key_value;
int input_rc_sub;
int optical_flow;
};
extern struct mavlink_subscriptions mavlink_subs;
apps/mavlink_onboard/Makefile
+44
View File
@@ -0,0 +1,44 @@
############################################################################
#
# Copyright (C) 2012 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.
#
############################################################################
#
# Mavlink Application
#
APPNAME = mavlink_onboard
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
include $(APPDIR)/mk/app.mk
apps/mavlink_onboard/mavlink.c
+529
View File
File diff suppressed because it is too large Load Diff
apps/mavlink_onboard/mavlink_bridge_header.h
+81
View File
@@ -0,0 +1,81 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.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 mavlink_bridge_header
* MAVLink bridge header for UART access.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
/* MAVLink adapter header */
#ifndef MAVLINK_BRIDGE_HEADER_H
#define MAVLINK_BRIDGE_HEADER_H
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
/* use efficient approach, see mavlink_helpers.h */
#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
#include "v1.0/mavlink_types.h"
#include <unistd.h>
/* Struct that stores the communication settings of this system.
you can also define / alter these settings elsewhere, as long
as they're included BEFORE mavlink.h.
So you can set the
mavlink_system.sysid = 100; // System ID, 1-255
mavlink_system.compid = 50; // Component/Subsystem ID, 1-255
Lines also in your main.c, e.g. by reading these parameter from EEPROM.
*/
extern mavlink_system_t mavlink_system;
/**
* @brief Send multiple chars (uint8_t) over a comm channel
*
* @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
* @param ch Character to send
*/
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
#endif /* MAVLINK_BRIDGE_HEADER_H */
apps/mavlink_onboard/mavlink_receiver.c
+332
View File
@@ -0,0 +1,332 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.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 mavlink_receiver.c
* MAVLink protocol message receive and dispatch
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
/* XXX trim includes */
#include <nuttx/config.h>
#include <unistd.h>
#include <pthread.h>
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <fcntl.h>
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <stdlib.h>
#include <poll.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include "util.h"
#include "orb_topics.h"
/* XXX should be in a header somewhere */
pthread_t receive_start(int uart);
static void handle_message(mavlink_message_t *msg);
static void *receive_thread(void *arg);
static mavlink_status_t status;
static struct vehicle_vicon_position_s vicon_position;
static struct vehicle_command_s vcmd;
static struct offboard_control_setpoint_s offboard_control_sp;
struct vehicle_global_position_s hil_global_pos;
struct vehicle_attitude_s hil_attitude;
orb_advert_t pub_hil_global_pos = -1;
orb_advert_t pub_hil_attitude = -1;
static orb_advert_t cmd_pub = -1;
static orb_advert_t flow_pub = -1;
static orb_advert_t offboard_control_sp_pub = -1;
static orb_advert_t vicon_position_pub = -1;
extern bool gcs_link;
static void
handle_message(mavlink_message_t *msg)
{
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */
printf("[mavlink] Terminating .. \n");
fflush(stdout);
usleep(50000);
/* terminate other threads and this thread */
thread_should_exit = true;
} else {
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
vcmd.param5 = cmd_mavlink.param5;
vcmd.param6 = cmd_mavlink.param6;
vcmd.param7 = cmd_mavlink.param7;
vcmd.command = cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
}
/* publish */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
}
if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
mavlink_optical_flow_t flow;
mavlink_msg_optical_flow_decode(msg, &flow);
struct optical_flow_s f;
f.timestamp = hrt_absolute_time();
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
f.flow_comp_y_m = flow.flow_comp_m_y;
f.ground_distance_m = flow.ground_distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
/* check if topic is advertised */
if (flow_pub <= 0) {
flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
/* publish */
orb_publish(ORB_ID(optical_flow), flow_pub, &f);
}
printf("GOT FLOW!\n");
}
if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
/* Set mode on request */
mavlink_set_mode_t new_mode;
mavlink_msg_set_mode_decode(msg, &new_mode);
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
vcmd.param2 = new_mode.custom_mode;
vcmd.param3 = 0;
vcmd.param4 = 0;
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
vcmd.command = MAV_CMD_DO_SET_MODE;
vcmd.target_system = new_mode.target_system;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* create command */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
/* Handle Vicon position estimates */
if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
vicon_position.x = pos.x;
vicon_position.y = pos.y;
vicon_position.z = pos.z;
if (vicon_position_pub <= 0) {
vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
}
}
/* Handle quadrotor motor setpoints */
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
if (mavlink_system.sysid < 4) {
/* switch to a receiving link mode */
gcs_link = false;
/*
* rate control mode - defined by MAVLink
*/
uint8_t ml_mode = 0;
bool ml_armed = false;
switch (quad_motors_setpoint.mode) {
case 0:
ml_armed = false;
break;
case 1:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
ml_armed = true;
break;
case 2:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
ml_armed = true;
break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
case 4:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
break;
}
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX;
offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX;
if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
ml_armed = false;
}
offboard_control_sp.armed = ml_armed;
offboard_control_sp.mode = ml_mode;
offboard_control_sp.timestamp = hrt_absolute_time();
/* check if topic has to be advertised */
if (offboard_control_sp_pub <= 0) {
offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
/* Publish */
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
}
}
}
}
/**
* Receive data from UART.
*/
static void *
receive_thread(void *arg)
{
int uart_fd = *((int*)arg);
const int timeout = 1000;
uint8_t ch;
mavlink_message_t msg;
prctl(PR_SET_NAME, "mavlink offb rcv", getpid());
while (!thread_should_exit) {
struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
if (poll(fds, 1, timeout) > 0) {
/* non-blocking read until buffer is empty */
int nread = 0;
do {
nread = read(uart_fd, &ch, 1);
if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
/* handle generic messages and commands */
handle_message(&msg);
}
} while (nread > 0);
}
}
return NULL;
}
pthread_t
receive_start(int uart)
{
pthread_attr_t receiveloop_attr;
pthread_attr_init(&receiveloop_attr);
struct sched_param param;
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2048);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
return thread;
}
apps/mavlink_onboard/orb_topics.h
+100
View File
@@ -0,0 +1,100 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.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 orb_topics.h
* Common sets of topics subscribed to or published by the MAVLink driver,
* and structures maintained by those subscriptions.
*/
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.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_attitude_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_rc_input.h>
struct mavlink_subscriptions {
int sensor_sub;
int att_sub;
int global_pos_sub;
int act_0_sub;
int act_1_sub;
int act_2_sub;
int act_3_sub;
int gps_sub;
int man_control_sp_sub;
int armed_sub;
int actuators_sub;
int local_pos_sub;
int spa_sub;
int spl_sub;
int spg_sub;
int debug_key_value;
int input_rc_sub;
};
extern struct mavlink_subscriptions mavlink_subs;
/** Global position */
extern struct vehicle_global_position_s global_pos;
/** Local position */
extern struct vehicle_local_position_s local_pos;
/** Vehicle status */
// extern struct vehicle_status_s v_status;
/** RC channels */
extern struct rc_channels_s rc;
/** Actuator armed state */
// extern struct actuator_armed_s armed;
/** Worker thread starter */
extern pthread_t uorb_receive_start(void);
apps/mavlink_onboard/util.h
+54
View File
@@ -0,0 +1,54 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.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 util.h
* Utility and helper functions and data.
*/
#pragma once
#include "orb_topics.h"
/** MAVLink communications channel */
extern uint8_t chan;
/** Shutdown marker */
extern volatile bool thread_should_exit;
/**
* Translate the custom state into standard mavlink modes and state.
*/
extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
uint8_t *mavlink_state, uint8_t *mavlink_mode);
apps/multirotor_att_control/multirotor_att_control_main.c
+92 -34
View File
@@ -69,10 +69,13 @@
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include "multirotor_attitude_control.h"
#include "multirotor_rate_control.h"
PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
static bool thread_should_exit;
@@ -102,6 +105,7 @@ mc_thread_main(int argc, char *argv[])
memset(&rates_sp, 0, sizeof(rates_sp));
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -148,6 +152,17 @@ mc_thread_main(int argc, char *argv[])
bool flag_control_attitude_enabled = false;
bool flag_system_armed = 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;
/* 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 */
@@ -198,31 +213,30 @@ mc_thread_main(int argc, char *argv[])
/** STEP 1: Define which input is the dominating control input */
if (state.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
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);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1;
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);
}
/* decide wether we want rate or position input */
}
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
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);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1;
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);
}
/* decide wether we want rate or position input */
}
else if (state.flag_control_manual_enabled) {
/* manual inputs, from RC control or joystick */
/* manual inputs, from RC control or joystick */
if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
rates_sp.roll = manual.roll;
@@ -241,18 +255,61 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = att.yaw;
}
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
static bool rc_loss_first_time = true;
/* only move setpoint if manual input is != 0 */
// XXX turn into param
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
} else if (manual.throttle <= 0.3f) {
att_sp.yaw_body = att.yaw;
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if(state.rc_signal_lost) {
/* 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.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.thrust = failsafe_throttle;
/* 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;
// XXX hard-limit it to prevent ballistic mishaps - this is just supposed to
// slow a crash down, not actually keep the system in-air.
rc_loss_first_time = false;
} else {
rc_loss_first_time = true;
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
/* set attitude if arming */
if (!flag_control_attitude_enabled && state.flag_system_armed) {
att_sp.yaw_body = att.yaw;
}
/* only move setpoint if manual input is != 0 */
if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) {
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
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;
}
control_yaw_position = true;
}
} else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
}
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 result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
@@ -272,7 +329,8 @@ mc_thread_main(int argc, char *argv[])
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
if (state.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
apps/multirotor_att_control/multirotor_attitude_control.c
+18 -5
View File
@@ -158,7 +158,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
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, struct actuator_controls_s *actuators)
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
@@ -181,6 +181,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
static bool initialized = false;
static float yaw_error;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
@@ -199,8 +201,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* update parameters from storage */
parameters_update(&h, &p);
//printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
/* 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);
@@ -216,9 +216,22 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
att->roll, att->rollspeed, deltaT);
/* control yaw rate */
rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed);
if(control_yaw_position) {
/* control yaw rate */
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
yaw_error = att_sp->yaw_body - att->yaw;
if (yaw_error > M_PI_F) {
yaw_error -= M_TWOPI_F;
} else if (yaw_error < -M_PI_F) {
yaw_error += M_TWOPI_F;
}
rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
}
rates_sp->thrust = att_sp->thrust;
motor_skip_counter++;
apps/multirotor_att_control/multirotor_attitude_control.h
+1 -1
View File
@@ -52,6 +52,6 @@
#include <uORB/topics/actuator_controls.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, struct actuator_controls_s *actuators);
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
apps/multirotor_att_control/multirotor_rate_control.c
+5
View File
@@ -209,6 +209,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
/* control yaw rate */
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
/* increase resilience to faulty control inputs */
if (!isfinite(yaw_rate_control)) {
yaw_rate_control = 0.0f;
warnx("rej. NaN ctrl yaw");
}
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
apps/px4/tests/test_sensors.c
+4 -4
View File
@@ -125,7 +125,7 @@ accel(int argc, char *argv[])
/* read data - expect samples */
ret = read(fd, &buf, sizeof(buf));
if (ret < 3) {
if (ret != sizeof(buf)) {
printf("\tACCEL: read1 fail (%d)\n", ret);
return ERROR;
@@ -177,7 +177,7 @@ gyro(int argc, char *argv[])
/* read data - expect samples */
ret = read(fd, &buf, sizeof(buf));
if (ret < 3) {
if (ret != sizeof(buf)) {
printf("\tGYRO: read fail (%d)\n", ret);
return ERROR;
@@ -214,7 +214,7 @@ mag(int argc, char *argv[])
/* read data - expect samples */
ret = read(fd, &buf, sizeof(buf));
if (ret < 3) {
if (ret != sizeof(buf)) {
printf("\tMAG: read fail (%d)\n", ret);
return ERROR;
@@ -251,7 +251,7 @@ baro(int argc, char *argv[])
/* read data - expect samples */
ret = read(fd, &buf, sizeof(buf));
if (ret < 3) {
if (ret != sizeof(buf)) {
printf("\tBARO: read fail (%d)\n", ret);
return ERROR;
apps/px4io/comms.c
+8 -2
View File
@@ -239,13 +239,19 @@ comms_handle_command(const void *buffer, size_t length)
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->servo_command[i];
/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
if(system_state.arm_ok && !cmd->arm_ok) {
system_state.armed = false;
}
system_state.arm_ok = cmd->arm_ok;
system_state.mixer_use_fmu = true;
system_state.fmu_data_received = true;
/* handle changes signalled by FMU */
if (!system_state.arm_ok && system_state.armed)
system_state.armed = false;
// if (!system_state.arm_ok && system_state.armed)
// system_state.armed = false;
/* XXX do relay changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
apps/px4io/mixer.c
+4 -3
View File
@@ -82,7 +82,7 @@ static void mixer_get_rc_input(void);
static void mixer_update(int mixer, uint16_t *inputs, int input_count);
/* current servo arm/disarm state */
bool mixer_servos_armed;
bool mixer_servos_armed = false;
/*
* Each mixer consumes a set of inputs and produces a single output.
@@ -159,7 +159,7 @@ mixer_tick(void *arg)
/*
* If we are armed, update the servo output.
*/
if (system_state.armed)
if (system_state.armed && system_state.arm_ok)
up_pwm_servo_set(i, mixers[i].current_value);
}
}
@@ -167,7 +167,8 @@ mixer_tick(void *arg)
/*
* Decide whether the servos should be armed right now.
*/
should_arm = system_state.armed && (control_count > 0);
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu;
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
apps/px4io/px4io.c
+2 -1
View File
@@ -133,8 +133,9 @@ int user_start(int argc, char *argv[])
/* print some simple status */
if (timers[TIMER_STATUS_PRINT] == 0) {
timers[TIMER_STATUS_PRINT] = 1000;
lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d \r",
lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d \r",
cursor[cycle++ & 3],
(system_state.arm_ok ? "FMU_ARMED" : "FMU_SAFE"),
(system_state.armed ? "ARMED" : "SAFE"),
(system_state.rc_channels ? "RC OK" : "NO RC"),
(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
apps/px4io/px4io.h
+1 -1
View File
@@ -68,7 +68,7 @@
struct sys_state_s
{
bool armed; /* actually armed */
bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */
/*
apps/px4io/safety.c
+29 -11
View File
@@ -56,11 +56,13 @@ static struct hrt_call arming_call;
* Count the number of times in a row that we see the arming button
* held down.
*/
static unsigned arm_counter;
static unsigned counter;
#define ARM_COUNTER_THRESHOLD 10
#define DISARM_COUNTER_THRESHOLD 2
static bool safety_led_state;
static bool safety_button_pressed;
static void safety_check_button(void *arg);
void
@@ -76,19 +78,35 @@ safety_check_button(void *arg)
/*
* Debounce the safety button, change state if it has been held for long enough.
*
* Ignore the button if FMU has not said it's OK to arm yet.
*/
if (BUTTON_SAFETY && system_state.arm_ok) {
if (arm_counter < ARM_COUNTER_THRESHOLD) {
arm_counter++;
} else if (arm_counter == ARM_COUNTER_THRESHOLD) {
/* change our armed state and notify the FMU */
system_state.armed = !system_state.armed;
arm_counter++;
safety_button_pressed = BUTTON_SAFETY;
if(safety_button_pressed) {
//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
}
/* Keep pressed for a while to arm */
if (safety_button_pressed && !system_state.armed) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to armed state and notify the FMU */
system_state.armed = true;
counter++;
system_state.fmu_report_due = true;
}
/* Disarm quickly */
} else if (safety_button_pressed && system_state.armed) {
if (counter < DISARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == DISARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
system_state.armed = false;
counter++;
system_state.fmu_report_due = true;
}
} else {
arm_counter = 0;
counter = 0;
}
/* when armed, toggle the LED; when safe, leave it on */
apps/sdlog/sdlog.c
+45 -6
View File
@@ -59,10 +59,13 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/vehicle_command.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/vehicle_vicon_position.h>
#include <uORB/topics/optical_flow.h>
#include <systemlib/systemlib.h>
@@ -295,10 +298,13 @@ int sdlog_thread_main(int argc, char *argv[]) {
struct vehicle_attitude_setpoint_s att_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct actuator_controls_effective_s act_controls_effective;
struct vehicle_command_s cmd;
struct vehicle_local_position_s local_pos;
struct vehicle_global_position_s global_pos;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -308,10 +314,13 @@ int sdlog_thread_main(int argc, char *argv[]) {
int att_sub;
int spa_sub;
int act_0_sub;
int controls0_sub;
int controls_0_sub;
int controls_effective_0_sub;
int local_pos_sub;
int global_pos_sub;
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
} subs;
/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -353,8 +362,15 @@ int sdlog_thread_main(int argc, char *argv[]) {
/* --- ACTUATOR CONTROL VALUE --- */
/* subscribe to ORB for actuator control */
subs.controls0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
fds[fdsc_count].fd = subs.controls0_sub;
subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
fds[fdsc_count].fd = subs.controls_0_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */
/* subscribe to ORB for actuator control */
subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
fds[fdsc_count].fd = subs.controls_effective_0_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -379,6 +395,20 @@ int sdlog_thread_main(int argc, char *argv[]) {
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VICON POSITION --- */
/* subscribe to ORB for vicon position */
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
fds[fdsc_count].fd = subs.vicon_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- FLOW measurements --- */
/* subscribe to ORB for flow measurements */
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
fds[fdsc_count].fd = subs.flow_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.
@@ -481,7 +511,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
@@ -489,6 +520,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
#pragma pack(push, 1)
struct {
@@ -507,6 +540,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
float attitude[3]; //pitch, roll, yaw [rad]
float rotMatrix[9]; //unitvectors
float vicon[6];
float control_effective[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
float flow[6]; // flow raw x, y, flow metric x, y, flow ground dist, flow quality
} sysvector = {
.timestamp = buf.raw.timestamp,
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@@ -523,13 +559,16 @@ int sdlog_thread_main(int argc, char *argv[]) {
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}
.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}
};
#pragma pack(pop)
sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
usleep(10000); //10000 corresponds in reality to ca. 76 Hz
usleep(3500); // roughly 150 Hz
}
fsync(sysvector_file);
apps/sensors/sensors.cpp
+17 -13
View File
@@ -151,6 +151,7 @@ private:
int _baro_sub; /**< raw baro data subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */
@@ -341,6 +342,7 @@ Sensors::Sensors() :
_baro_sub(-1),
_vstatus_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
/* publications */
_sensor_pub(-1),
@@ -903,6 +905,9 @@ Sensors::ppm_poll()
struct manual_control_setpoint_s manual_control;
/* get a copy first, to prevent altering values */
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control);
/* require at least four channels to consider the signal valid */
if (rc_input.channel_count < 4)
return;
@@ -1023,6 +1028,7 @@ Sensors::task_main()
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
@@ -1052,20 +1058,18 @@ Sensors::task_main()
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* advertise the manual_control topic */
{
struct manual_control_setpoint_s manual_control;
manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE;
manual_control.roll = 0.0f;
manual_control.pitch = 0.0f;
manual_control.yaw = 0.0f;
manual_control.throttle = 0.0f;
manual_control.aux1_cam_pan_flaps = 0.0f;
manual_control.aux2_cam_tilt = 0.0f;
manual_control.aux3_cam_zoom = 0.0f;
manual_control.aux4_cam_roll = 0.0f;
struct manual_control_setpoint_s manual_control;
manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS;
manual_control.roll = 0.0f;
manual_control.pitch = 0.0f;
manual_control.yaw = 0.0f;
manual_control.throttle = 0.0f;
manual_control.aux1_cam_pan_flaps = 0.0f;
manual_control.aux2_cam_tilt = 0.0f;
manual_control.aux3_cam_zoom = 0.0f;
manual_control.aux4_cam_roll = 0.0f;
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
}
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
/* advertise the rc topic */
{
apps/systemcmds/preflight_check/Makefile
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (C) 2012 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.
#
############################################################################
#
# Reboot command.
#
APPNAME = preflight_check
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/systemcmds/preflight_check/preflight_check.c
+198
View File
@@ -0,0 +1,198 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.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 reboot.c
* Tool similar to UNIX reboot command
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <errno.h>
#include <systemlib/err.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_baro.h>
__EXPORT int preflight_check_main(int argc, char *argv[]);
static int led_toggle(int leds, int led);
static int led_on(int leds, int led);
static int led_off(int leds, int led);
int preflight_check_main(int argc, char *argv[])
{
bool fail_on_error = false;
if (argc > 1 && !strcmp(argv[1], "--help")) {
warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error");
exit(1);
}
if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) {
fail_on_error = true;
}
bool system_ok = true;
int fd;
int ret;
/* give the system some time to sample the sensors in the background */
usleep(150000);
/* ---- MAG ---- */
close(fd);
fd = open(MAG_DEVICE_PATH, 0);
if (fd < 0) {
warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
system_ok = false;
goto system_eval;
}
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
system_ok = false;
goto system_eval;
}
/* ---- ACCEL ---- */
close(fd);
fd = open(ACCEL_DEVICE_PATH, 0);
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
warnx("accel self test failed");
system_ok = false;
goto system_eval;
}
/* ---- GYRO ---- */
close(fd);
fd = open(GYRO_DEVICE_PATH, 0);
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) {
warnx("gyro self test failed");
system_ok = false;
goto system_eval;
}
/* ---- BARO ---- */
close(fd);
fd = open(BARO_DEVICE_PATH, 0);
system_eval:
if (system_ok) {
/* all good, exit silently */
exit(0);
} else {
fflush(stdout);
int buzzer = open("/dev/tone_alarm", O_WRONLY);
int leds = open(LED_DEVICE_PATH, 0);
/* flip blue led into alternating amber */
led_off(leds, LED_BLUE);
led_off(leds, LED_AMBER);
led_toggle(leds, LED_BLUE);
/* display and sound error */
for (int i = 0; i < 200; i++)
{
led_toggle(leds, LED_BLUE);
led_toggle(leds, LED_AMBER);
if (i % 10 == 0) {
ioctl(buzzer, TONE_SET_ALARM, 4);
} else if (i % 5 == 0) {
ioctl(buzzer, TONE_SET_ALARM, 2);
}
usleep(100000);
}
/* stop alarm */
ioctl(buzzer, TONE_SET_ALARM, 0);
/* switch on leds */
led_on(leds, LED_BLUE);
led_on(leds, LED_AMBER);
if (fail_on_error) {
/* exit with error message */
exit(1);
} else {
/* do not emit an error code to make sure system still boots */
exit(0);
}
}
}
static int led_toggle(int leds, int led)
{
static int last_blue = LED_ON;
static int last_amber = LED_ON;
if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
}
static int led_off(int leds, int led)
{
return ioctl(leds, LED_OFF, led);
}
static int led_on(int leds, int led)
{
return ioctl(leds, LED_ON, led);
}
apps/systemlib/geo/geo.c
+38 -38
View File
@@ -60,14 +60,7 @@ static double cos_phi_1;
static double lambda_0;
static double scale;
/**
* Initializes the map transformation.
*
* Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
phi_1 = lat_0 / 180.0 * M_PI;
@@ -105,14 +98,7 @@ __EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lo
}
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y)
__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
double phi = lat / 180.0 * M_PI;
@@ -135,15 +121,7 @@ __EXPORT static void map_projection_project(double lat, double lon, float *x, fl
// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
}
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon)
__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
@@ -228,7 +206,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
/* conscious mix of double and float trig function to maximize speed and efficiency */
float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
theta = _wrapPI(theta);
theta = _wrap_pi(theta);
return theta;
}
@@ -257,7 +235,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
bearing_diff = bearing_track - bearing_end;
bearing_diff = _wrapPI(bearing_diff);
bearing_diff = _wrap_pi(bearing_diff);
// Return past_end = true if past end point of line
if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
@@ -270,10 +248,10 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
if (sin(bearing_diff) >= 0) {
crosstrack_error->bearing = _wrapPI(bearing_track - M_PI_2_F);
crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
} else {
crosstrack_error->bearing = _wrapPI(bearing_track + M_PI_2_F);
crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
}
return_value = OK;
@@ -380,22 +358,36 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
return return_value;
}
float _wrapPI(float bearing)
__EXPORT float _wrap_pi(float bearing)
{
while (bearing > M_PI_F) {
bearing = bearing - M_TWOPI_F;
/* value is inf or NaN */
if (!isfinite(bearing) || bearing == 0) {
return bearing;
}
while (bearing <= -M_PI_F) {
bearing = bearing + M_TWOPI_F;
int c = 0;
while (bearing > M_PI_F && c < 30) {
bearing -= M_TWOPI_F;
c++;
}
c = 0;
while (bearing <= -M_PI_F && c < 30) {
bearing += M_TWOPI_F;
c++;
}
return bearing;
}
float _wrap2PI(float bearing)
__EXPORT float _wrap_2pi(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
while (bearing >= M_TWOPI_F) {
bearing = bearing - M_TWOPI_F;
@@ -408,8 +400,12 @@ float _wrap2PI(float bearing)
return bearing;
}
float _wrap180(float bearing)
__EXPORT float _wrap_180(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
while (bearing > 180.0f) {
bearing = bearing - 360.0f;
@@ -422,8 +418,12 @@ float _wrap180(float bearing)
return bearing;
}
float _wrap360(float bearing)
__EXPORT float _wrap_360(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
while (bearing >= 360.0f) {
bearing = bearing - 360.0f;
apps/systemlib/geo/geo.h
+29 -9
View File
@@ -54,24 +54,44 @@ struct crosstrack_error_s {
float bearing; // Bearing in radians to closest point on line/arc
} ;
__EXPORT static void map_projection_init(double lat_0, double lon_0);
/**
* Initializes the map transformation.
*
* Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT void map_projection_init(double lat_0, double lon_0);
__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y);
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon);
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
//
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep);
float _wrap180(float bearing);
float _wrap360(float bearing);
float _wrapPI(float bearing);
float _wrap2PI(float bearing);
__EXPORT float _wrap_180(float bearing);
__EXPORT float _wrap_360(float bearing);
__EXPORT float _wrap_pi(float bearing);
__EXPORT float _wrap_2pi(float bearing);
apps/systemlib/mixer/mixer_multirotor.cpp
+22
View File
@@ -161,6 +161,28 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float max = 0.0f;
float fixup_scale;
/* use an output factor to prevent too strong control signals at low throttle */
float min_thrust = 0.05f;
float max_thrust = 1.0f;
float startpoint_full_control = 0.40f;
float output_factor;
/* keep roll, pitch and yaw control to 0 below min thrust */
if (thrust <= min_thrust) {
output_factor = 0.0f;
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
} else if (thrust < startpoint_full_control && thrust > min_thrust) {
output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust);
/* and then stay at full control */
} else {
output_factor = max_thrust;
}
roll *= output_factor;
pitch *= output_factor;
yaw *= output_factor;
/* perform initial mix pass yielding un-bounded outputs */
for (unsigned i = 0; i < _rotor_count; i++) {
float tmp = roll * _rotors[i].roll_scale +
apps/systemlib/param/param.c
+1 -3
View File
@@ -517,13 +517,11 @@ param_save_default(void)
}
int result = param_export(fd, false);
/* should not be necessary, over-careful here */
fsync(fd);
close(fd);
if (result != 0) {
unlink(param_get_default_file());
warn("error exporting parameters to '%s'", param_get_default_file());
unlink(param_get_default_file());
return -2;
}
apps/systemlib/param/param.h
+1 -1
View File
@@ -250,7 +250,7 @@ __EXPORT int param_set_default_file(const char* filename);
* a result of a call to param_set_default_file, or the
* built-in default.
*/
__EXPORT const char *param_get_default_file(void);
__EXPORT const char* param_get_default_file(void);
/**
* Save parameters to the default file.

Some files were not shown because too many files have changed in this diff Show More