fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 10:27:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master into mpc_in_flight_lock

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-30 17:03:02 +02:00
parent 8a25c48071 3c5f35da73
commit 478f923331
158 changed files with 10685 additions and 4407 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/blinkm/blinkm.cpp
+3 -2
View File
@@ -655,13 +655,14 @@ BlinkM::led()
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
led_color_4 = LED_GREEN;
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
/* TODO: add other Auto modes */
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
else
else
led_color_4 = LED_OFF;
led_color_5 = led_color_4;
}
src/drivers/boards/px4fmu-v1/px4fmu_init.c
+1 -1
View File
@@ -99,7 +99,7 @@
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
src/drivers/boards/px4fmu-v1/px4fmu_led.c
+2 -2
View File
@@ -54,13 +54,13 @@
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);
__END_DECLS
__EXPORT void led_init()
__EXPORT void led_init(void)
{
/* Configure LED1-2 GPIOs for output */
src/drivers/boards/px4fmu-v2/px4fmu2_led.c
+1 -1
View File
@@ -54,7 +54,7 @@
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);
src/drivers/ets_airspeed/ets_airspeed.cpp
+2 -2
View File
@@ -364,7 +364,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %d pa", report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -389,7 +389,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("diff pressure: %d pa", report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
}
/* reset the sensor polling to its default rate */
src/drivers/gps/gps.cpp
+6 -6
View File
@@ -280,8 +280,8 @@ GPS::task_main()
_report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f;
_report.fix_type = 3;
_report.eph_m = 0.9f;
_report.epv_m = 1.8f;
_report.eph = 0.9f;
_report.epv = 1.8f;
_report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f;
@@ -357,7 +357,7 @@ GPS::task_main()
}
if (!_healthy) {
char *mode_str = "unknown";
const char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
@@ -449,9 +449,9 @@ GPS::print_info()
if (_report.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -578,7 +578,7 @@ gps_main(int argc, char *argv[])
{
/* set to default */
char *device_name = GPS_DEFAULT_UART_PORT;
const char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false;
/*
src/drivers/gps/mtk.cpp
+4 -4
View File
@@ -251,16 +251,16 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->lon = 0;
// Indicate this data is not usable and bail out
_gps_position->eph_m = 1000.0f;
_gps_position->epv_m = 1000.0f;
_gps_position->eph = 1000.0f;
_gps_position->epv = 1000.0f;
_gps_position->fix_type = 0;
return;
}
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
_gps_position->fix_type = packet.fix_type;
_gps_position->eph_m = packet.hdop / 100.0f; // from cm to m
_gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph
_gps_position->eph = packet.hdop / 100.0f; // from cm to m
_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_visible = packet.satellites;
src/drivers/gps/ubx.cpp
+2 -2
View File
@@ -439,8 +439,8 @@ UBX::handle_message()
_gps_position->lat = packet->lat;
_gps_position->lon = packet->lon;
_gps_position->alt = packet->height_msl;
_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
_gps_position->eph = (float)packet->hAcc * 1e-3f; // from mm to m
_gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m
_gps_position->timestamp_position = hrt_absolute_time();
_rate_count_lat_lon++;
src/drivers/hmc5883/hmc5883.cpp
+2 -2
View File
@@ -326,9 +326,9 @@ HMC5883::HMC5883(int bus) :
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_collect_phase(false),
_class_instance(-1),
_mag_topic(-1),
_subsystem_pub(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -1228,7 +1228,7 @@ HMC5883::print_info()
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
(double)1.0/_range_scale, (double)_range_ga);
(double)(1.0f/_range_scale), (double)_range_ga);
_reports->print_info("report queue");
}
src/drivers/mkblctrl/mkblctrl.cpp
+12 -17
View File
@@ -131,8 +131,8 @@ public:
int set_motor_count(unsigned count);
int set_motor_test(bool motortest);
int set_overrideSecurityChecks(bool overrideSecurityChecks);
int set_px4mode(int px4mode);
int set_frametype(int frametype);
void set_px4mode(int px4mode);
void set_frametype(int frametype);
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
private:
@@ -222,15 +222,15 @@ MK::MK(int bus, const char *_device_path) :
_task(-1),
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_esc_status(0),
_num_outputs(0),
_motortest(false),
_overrideSecurityChecks(false),
_motor(-1),
_px4mode(MAPPING_MK),
_frametype(FRAME_PLUS),
_t_outputs(0),
_t_esc_status(0),
_num_outputs(0),
_primary_pwm_device(false),
_motortest(false),
_overrideSecurityChecks(false),
_task_should_exit(false),
_armed(false),
_mixers(nullptr)
@@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate)
return OK;
}
int
void
MK::set_px4mode(int px4mode)
{
_px4mode = px4mode;
}
int
void
MK::set_frametype(int frametype)
{
_frametype = frametype;
@@ -440,9 +440,6 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
void
MK::task_main()
{
long update_rate_in_us = 0;
float tmpVal = 0;
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
@@ -483,7 +480,6 @@ MK::task_main()
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
update_rate_in_us = long(1000000 / _update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
@@ -735,7 +731,6 @@ MK::mk_servo_set(unsigned int chan, short val)
_retries = 0;
uint8_t result[3] = { 0, 0, 0 };
uint8_t msg[2] = { 0, 0 };
uint8_t rod = 0;
uint8_t bytesToSendBL2 = 2;
tmpVal = val;
@@ -824,7 +819,7 @@ MK::mk_servo_set(unsigned int chan, short val)
if (debugCounter == 2000) {
debugCounter = 0;
for (int i = 0; i < _num_outputs; i++) {
for (unsigned int i = 0; i < _num_outputs; i++) {
if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
}
@@ -1169,7 +1164,7 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr
}
int
mk_start(unsigned motors, char *device_path)
mk_start(unsigned motors, const char *device_path)
{
int ret;
@@ -1228,7 +1223,7 @@ mkblctrl_main(int argc, char *argv[])
bool overrideSecurityChecks = false;
bool showHelp = false;
bool newMode = false;
char *devicepath = "";
const char *devicepath = "";
/*
* optional parameters
src/drivers/mpu6000/mpu6000.cpp
+1 -1
View File
@@ -544,7 +544,7 @@ void MPU6000::reset()
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
up_udelay(1000);
usleep(1000);
// SAMPLE RATE
_set_sample_rate(_sample_rate);
src/drivers/px4fmu/fmu.cpp
+7 -7
View File
@@ -240,8 +240,6 @@ PX4FMU::PX4FMU() :
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_task(-1),
_control_subs({-1}),
_poll_fds_num(0),
_armed_sub(-1),
_outputs_pub(-1),
_num_outputs(0),
@@ -252,10 +250,12 @@ PX4FMU::PX4FMU() :
_mixers(nullptr),
_groups_required(0),
_groups_subscribed(0),
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
_control_subs{-1},
_poll_fds_num(0),
_failsafe_pwm{0},
_disarmed_pwm{0},
_num_failsafe_set(0),
_num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
@@ -741,7 +741,7 @@ PX4FMU::task_main()
}
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs > 0) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
}
src/drivers/px4io/px4io.cpp
+35 -17
View File
@@ -72,6 +72,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/param/param.h>
#include <systemlib/circuit_breaker.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@@ -197,8 +198,10 @@ public:
* Print IO status.
*
* Print all relevant IO status information
*
* @param extended_status Shows more verbose information (in particular RC config)
*/
void print_status();
void print_status(bool extended_status);
/**
* Fetch and print debug console output.
@@ -1010,6 +1013,19 @@ PX4IO::task_main()
}
}
int32_t safety_param_val;
param_t safety_param = param_find("RC_FAILS_THR");
if (safety_param != PARAM_INVALID) {
param_get(safety_param, &safety_param_val);
if (safety_param_val == PX4IO_FORCE_SAFETY_MAGIC) {
/* disable IO safety if circuit breaker asked for it */
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, safety_param_val);
}
}
}
}
@@ -1850,7 +1866,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
}
void
PX4IO::print_status()
PX4IO::print_status(bool extended_status)
{
/* basic configuration */
printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
@@ -2013,19 +2029,21 @@ PX4IO::print_status()
printf("\n");
}
for (unsigned i = 0; i < _max_rc_input; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
i,
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
options,
((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
if (extended_status) {
for (unsigned i = 0; i < _max_rc_input; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
i,
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
options,
((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
}
}
printf("failsafe");
@@ -2853,7 +2871,7 @@ monitor(void)
if (g_dev != nullptr) {
printf("\033[2J\033[H"); /* move cursor home and clear screen */
(void)g_dev->print_status();
(void)g_dev->print_status(false);
(void)g_dev->print_debug();
printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
@@ -3119,7 +3137,7 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
printf("[px4io] loaded\n");
g_dev->print_status();
g_dev->print_status(true);
exit(0);
}
src/drivers/px4io/px4io_serial.cpp
+1 -1
View File
@@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt()
if (_rx_dma_status == _dma_status_waiting) {
/* verify that the received packet is complete */
int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
perf_count(_pc_badidle);
src/drivers/px4io/px4io_uploader.cpp
+3 -3
View File
@@ -240,9 +240,9 @@ PX4IO_Uploader::upload(const char *filenames[])
close(_io_fd);
_io_fd = -1;
// sleep for enough time for the IO chip to boot. This makes
// forceupdate more reliably startup IO again after update
up_udelay(100*1000);
// sleep for enough time for the IO chip to boot. This makes
// forceupdate more reliably startup IO again after update
up_udelay(100*1000);
return ret;
}
src/drivers/roboclaw/RoboClaw.cpp
+8 -2
View File
@@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor)
return _motor1Position;
} else if (motor == MOTOR_2) {
return _motor2Position;
}
} else {
warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
return NAN;
}
}
float RoboClaw::getMotorSpeed(e_motor motor)
@@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor)
return _motor1Speed;
} else if (motor == MOTOR_2) {
return _motor2Speed;
}
} else {
warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
return NAN;
}
}
int RoboClaw::setMotorSpeed(e_motor motor, float value)
src/drivers/sf0x/sf0x.cpp
+1 -1
View File
@@ -616,7 +616,7 @@ SF0X::collect()
}
}
debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO"));
debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
/* done with this chunk, resetting - even if invalid */
_linebuf_index = 0;
src/examples/px4_simple_app/px4_simple_app.c
+1
View File
@@ -41,6 +41,7 @@
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
-3
View File
@@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
-3
View File
@@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch,
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0; //xxx: param
/* input conditioning */
// warnx("airspeed pre %.4f", (double)airspeed);
if (!isfinite(airspeed)) {
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+1 -6
View File
@@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch,
_rate_setpoint = 0.0f;
if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
if(denumerator != 0.0f) { //XXX: floating point comparison
if(fabsf(denumerator) > FLT_EPSILON) {
_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
}
@@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
src/lib/geo/geo.c
+16 -17
View File
@@ -82,8 +82,8 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
{
float x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
float y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
@@ -146,7 +146,6 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
@@ -174,8 +173,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
*lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
*lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
}
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
@@ -197,7 +196,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
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);
@@ -212,7 +211,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
}
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
if (sin(bearing_diff) >= 0) {
crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
@@ -248,7 +247,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; }
if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
if (arc_sweep >= 0) {
@@ -296,14 +295,14 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
// as this function generally will not be called repeatedly when we are out of the sector.
// TO DO - this is messed up and won't compile
float start_disp_x = radius * sin(arc_start_bearing);
float start_disp_y = radius * cos(arc_start_bearing);
float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
float lon_start = lon_now + start_disp_x / 111111.0d;
float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
float lon_end = lon_now + end_disp_x / 111111.0d;
float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
float start_disp_x = radius * sinf(arc_start_bearing);
float start_disp_y = radius * cosf(arc_start_bearing);
float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep));
float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep));
float lon_start = lon_now + start_disp_x / 111111.0f;
float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f;
float lon_end = lon_now + end_disp_x / 111111.0f;
float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f;
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
@@ -337,7 +336,7 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now
double d_lat = x_rad - current_x_rad;
double d_lon = y_rad - current_y_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
src/lib/geo/geo.h
+1 -1
View File
@@ -50,7 +50,7 @@
__BEGIN_DECLS
#include "geo/geo_mag_declination.h"
#include "geo_lookup/geo_mag_declination.h"
#include <stdbool.h>
src/lib/geo/module.mk
+2 -3
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,5 +35,4 @@
# Geo library
#
SRCS = geo.c \
geo_mag_declination.c
SRCS = geo.c
src/lib/geo/geo_mag_declination.c → src/lib/geo_lookup/geo_mag_declination.c
+13 -18
View File
@@ -54,24 +54,19 @@
static const int8_t declination_table[13][37] = \
{
46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \
-66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \
-3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \
29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \
-40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \
8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \
10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \
-10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \
-14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \
9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \
7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \
0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \
0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \
0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \
-16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \
12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \
0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \
13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3
{ 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 },
{ 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 },
{ 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 },
{ 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 },
{ 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 },
{ 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 },
{ 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 },
{ 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 },
{ 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 },
{ 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 },
{ 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 },
{ 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 },
{ 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 },
};
static float get_lookup_table_val(unsigned lat, unsigned lon);
src/lib/geo/geo_mag_declination.h → src/lib/geo_lookup/geo_mag_declination.h
View File
src/lib/geo_lookup/module.mk
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Geo lookup table / data library
#
SRCS = geo_mag_declination.c
MAXOPTIMIZATION = -Os
src/lib/launchdetection/module.mk
+2
View File
@@ -38,3 +38,5 @@
SRCS = LaunchDetector.cpp \
CatapultLaunchMethod.cpp \
launchdetection_params.c
MAXOPTIMIZATION = -Os
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+23 -7
View File
@@ -65,6 +65,7 @@
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
@@ -287,6 +288,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
/* magnetic declination, in radians */
float mag_decl = 0.0f;
/* rotation matrix for magnetic declination */
math::Matrix<3, 3> R_decl;
R_decl.identity();
@@ -325,9 +329,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* update parameters */
parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
}
/* only run filter if sensor values changed */
@@ -340,6 +341,13 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
orb_check(sub_gps, &gps_updated);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, mag_decl);
}
}
bool global_pos_updated;
@@ -393,7 +401,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
hrt_abstime vel_t = 0;
bool vel_valid = false;
if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
vel_valid = true;
if (gps_updated) {
vel_t = gps.timestamp_velocity;
@@ -402,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
vel(2) = gps.vel_d_m_s;
}
} else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
} else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;
@@ -469,7 +477,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
} else {
mag_decl = ekf_params.mag_decl;
}
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, mag_decl);
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
@@ -515,7 +531,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2] + ekf_params.mag_decl;
att.yaw = euler[2] + mag_decl;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+1 -14
View File
@@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/* offset estimation - UNUSED */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
/* magnetic declination, in degrees */
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
@@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->r2 = param_find("EKF_ATT_V4_R2");
h->r3 = param_find("EKF_ATT_V4_R3");
h->roll_off = param_find("ATT_ROLL_OFF3");
h->pitch_off = param_find("ATT_PITCH_OFF3");
h->yaw_off = param_find("ATT_YAW_OFF3");
h->mag_decl = param_find("ATT_MAG_DECL");
h->acc_comp = param_find("ATT_ACC_COMP");
@@ -109,12 +100,8 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->r2, &(p->r[2]));
param_get(h->r3, &(p->r[3]));
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
param_get(h->mag_decl, &(p->mag_decl));
p->mag_decl *= M_PI / 180.0f;
p->mag_decl *= M_PI_F / 180.0f;
param_get(h->acc_comp, &(p->acc_comp));
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
-1
View File
@@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params {
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3;
param_t q0, q1, q2, q3, q4;
param_t roll_off, pitch_off, yaw_off;
param_t mag_decl;
param_t acc_comp;
};
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+4 -17
View File
@@ -392,8 +392,6 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
*/
int attitude_estimator_so3_thread_main(int argc, char *argv[])
{
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Time constant
float dt = 0.005f;
@@ -438,11 +436,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
/* keep track of sensor updates */
@@ -513,7 +509,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]);
}
} else {
@@ -523,12 +519,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
@@ -538,8 +531,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* update accelerometer measurements */
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
}
@@ -549,8 +540,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* update magnetometer measurements */
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
@@ -569,8 +558,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
continue;
}
uint64_t timing_start = hrt_absolute_time();
// NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
@@ -609,9 +596,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* due to inputs or numerical failure the output is invalid, skip it */
// Due to inputs or numerical failure the output is invalid
warnx("infinite euler angles, rotation matrix:");
warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]);
warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]);
warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]);
// Don't publish anything
continue;
}
src/modules/commander/accelerometer_calibration.cpp
+6 -3
View File
@@ -131,6 +131,7 @@
#include <fcntl.h>
#include <sys/prctl.h>
#include <math.h>
#include <float.h>
#include <mathlib/mathlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
@@ -158,6 +159,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
int do_accel_calibration(int mavlink_fd)
{
int fd;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
struct accel_scale accel_scale = {
@@ -172,7 +175,7 @@ int do_accel_calibration(int mavlink_fd)
int res = OK;
/* reset all offsets to zero and all scales to one */
int fd = open(ACCEL_DEVICE_PATH, 0);
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
@@ -223,7 +226,7 @@ int do_accel_calibration(int mavlink_fd)
if (res == OK) {
/* apply new scaling and offsets */
int fd = open(ACCEL_DEVICE_PATH, 0);
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
@@ -524,7 +527,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0f) {
if (fabsf(det) < FLT_EPSILON) {
return ERROR; // Singular matrix
}
src/modules/commander/calibration_routines.cpp
+5 -4
View File
@@ -40,6 +40,7 @@
*/
#include <math.h>
#include <float.h>
#include "calibration_routines.h"
@@ -170,7 +171,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
//Iterate N times, ignore stop condition.
int n = 0;
unsigned int n = 0;
while (n < max_iterations) {
n++;
@@ -179,9 +180,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
aA = (aA == 0.0f) ? 1.0f : aA;
aB = (aB == 0.0f) ? 1.0f : aB;
aC = (aC == 0.0f) ? 1.0f : aC;
aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
//Compute next iteration
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
src/modules/commander/commander.cpp
+374 -327
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander_params.c
+12 -1
View File
@@ -39,7 +39,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
@@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
/**
* Datalink loss mode enabled.
*
* Set to 1 to enable actions triggered when the datalink is lost.
*
* @group commander
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
src/modules/commander/px4_custom_mode.h
+1
View File
@@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
PX4_CUSTOM_SUB_MODE_AUTO_RTL,
PX4_CUSTOM_SUB_MODE_AUTO_LAND,
PX4_CUSTOM_SUB_MODE_AUTO_RTGS
};
union px4_custom_mode {
src/modules/commander/state_machine_helper.cpp
+226 -342
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,6 +34,9 @@
/**
* @file state_machine_helper.cpp
* State machine helper functions implementations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#include <stdio.h>
@@ -59,30 +60,20 @@
#include "state_machine_helper.h"
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool failsafe_state_changed = true;
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
@@ -144,6 +135,26 @@ arming_state_transition(struct vehicle_status_s *status, /// current
valid_transition = false;
}
// Perform power checks only if circuit breaker is not
// engaged for these checks
if (!status->circuit_breaker_engaged_power_check) {
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
valid_transition = false;
}
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
(status->avionics_power_rail_voltage < 4.9f)) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage);
valid_transition = false;
}
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
}
@@ -165,7 +176,6 @@ arming_state_transition(struct vehicle_status_s *status, /// current
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
}
@@ -199,69 +209,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
}
}
bool
check_arming_state_changed()
{
if (arming_state_changed) {
arming_state_changed = false;
return true;
} else {
return false;
}
}
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* transition may be denied even if requested the same state because conditions may be changed */
/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case MAIN_STATE_MANUAL:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
(status->condition_local_altitude_valid ||
status->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO:
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO_RTL:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_MAX:
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state = new_main_state;
main_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
@@ -270,70 +269,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
return ret;
}
bool
check_main_state_changed()
{
if (main_state_changed) {
main_state_changed = false;
return true;
} else {
return false;
}
}
bool
check_failsafe_state_changed()
{
if (failsafe_state_changed) {
failsafe_state_changed = false;
return true;
} else {
return false;
}
}
/**
* Transition from one hil state to another
*/
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
bool valid_transition = false;
int ret = ERROR;
transition_result_t ret = TRANSITION_DENIED;
if (current_status->hil_state == new_state) {
valid_transition = true;
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_state) {
case HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
valid_transition = false;
ret = TRANSITION_DENIED;
break;
case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
// Disable publication of all attached sensors
/* Disable publication of all attached sensors */
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
@@ -388,290 +352,210 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
return 1;
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
} else {
mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
default:
warnx("Unknown hil state");
warnx("Unknown HIL state");
break;
}
}
if (valid_transition) {
if (ret == TRANSITION_CHANGED) {
current_status->hil_state = new_state;
current_status->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// XXX also set lockdown here
ret = OK;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
}
return ret;
}
/**
* Transition from one failsafe state to another
*/
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
{
transition_result_t ret = TRANSITION_DENIED;
navigation_state_t nav_state_old = status->nav_state;
/* transition may be denied even if requested the same state because conditions may be changed */
if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
/* transitions from TERMINATION to other states not allowed */
if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
ret = TRANSITION_NOT_CHANGED;
}
bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
status->failsafe = false;
} else {
switch (new_failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* always allowed (except from TERMINATION state) */
ret = TRANSITION_CHANGED;
break;
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case MAIN_STATE_ACRO:
case MAIN_STATE_MANUAL:
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if (status->rc_signal_lost && armed) {
status->failsafe = true;
case FAILSAFE_STATE_RTL:
/* global position and home position required for RTL */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->set_nav_state = NAV_STATE_RTL;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
}
break;
case FAILSAFE_STATE_LAND:
/* at least relative altitude estimate required for landing */
if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
status->set_nav_state = NAV_STATE_LAND;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
}
break;
case FAILSAFE_STATE_TERMINATION:
/* always allowed */
ret = TRANSITION_CHANGED;
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->failsafe_state != new_failsafe_state) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
ret = TRANSITION_NOT_CHANGED;
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
} else {
switch (status->main_state) {
case MAIN_STATE_ACRO:
status->nav_state = NAVIGATION_STATE_ACRO;
break;
case MAIN_STATE_MANUAL:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
case MAIN_STATE_ALTCTL:
status->nav_state = NAVIGATION_STATE_ALTCTL;
break;
case MAIN_STATE_POSCTL:
status->nav_state = NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if either the datalink is enabled and lost as well as RC is lost
* - if there is no datalink and the mission is finished */
if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
(!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost and mission is not yet finished */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for missions */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
/* go into failsafe if datalink and RC is lost */
if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up */
} else if (status->rc_signal_lost && !data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost if datalink is connected */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for loitering */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
break;
case MAIN_STATE_AUTO_RTL:
/* require global position and home */
if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
}
break;
default:
break;
}
return ret;
return status->nav_state != nav_state_old;
}
// /*
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
// */
// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
// *
// * START SUBSYSTEM/EMERGENCY FUNCTIONS
// * */
// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was removed something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was disabled something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
//
//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
//{
// int ret = 1;
//
//// /* Switch on HIL if in standby and not already in HIL mode */
//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
//// && !current_status->flag_hil_enabled) {
//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
//// /* Enable HIL on request */
//// current_status->flag_hil_enabled = true;
//// ret = OK;
//// state_machine_publish(status_pub, current_status, mavlink_fd);
//// publish_armed_status(current_status);
//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
////
//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
//// current_status->flag_fmu_armed) {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
////
//// } else {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
//// }
//// }
//
// /* switch manual / auto */
// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
// }
//
// /* vehicle is disarmed, mode requests arming */
// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only arm in standby state */
// // XXX REMOVE
// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
// ret = OK;
// printf("[cmd] arming due to command request\n");
// }
// }
//
// /* vehicle is armed, mode requests disarming */
// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only disarm in ground ready */
// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
// ret = OK;
// printf("[cmd] disarming due to command request\n");
// }
// }
//
// /* NEVER actually switch off HIL without reboot */
// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
// ret = ERROR;
// }
//
// return ret;
//}
src/modules/commander/state_machine_helper.h
+4 -14
View File
@@ -56,25 +56,15 @@ typedef enum {
} transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
bool check_arming_state_changed();
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
bool check_main_state_changed();
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
bool check_navigation_state_changed();
bool check_failsafe_state_changed();
void set_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
#endif /* STATE_MACHINE_HELPER_H_ */
src/modules/dataman/dataman.c
+2 -1
View File
@@ -50,6 +50,7 @@
#include <string.h>
#include "dataman.h"
#include <systemlib/param/param.h>
/**
* data manager app start / stop handling function
@@ -187,7 +188,7 @@ create_work_item(void)
if (item) {
item->first = 1;
lock_queue(&g_free_q);
for (int i = 1; i < k_work_item_allocation_chunk_size; i++) {
for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) {
(item + i)->first = 0;
sq_addfirst(&(item + i)->link, &(g_free_q.q));
}
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+615 -404
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf_att_pos_estimator/estimator_21states.cpp
+2142
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf_att_pos_estimator/estimator_21states.h
+247
View File
@@ -0,0 +1,247 @@
#pragma once
#include "estimator_utilities.h"
class AttPosEKF {
public:
AttPosEKF();
~AttPosEKF();
/* ##############################################
*
* M A I N F I L T E R P A R A M E T E R S
*
* ########################################### */
/*
* parameters are defined here and initialised in
* the InitialiseParameters() (which is just 20 lines down)
*/
float covTimeStepMax; // maximum time allowed between covariance predictions
float covDelAngMax; // maximum delta angle between covariance predictions
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
float yawVarScale;
float windVelSigma;
float dAngBiasSigma;
float dVelBiasSigma;
float magEarthSigma;
float magBodySigma;
float gndHgtSigma;
float vneSigma;
float vdSigma;
float posNeSigma;
float posDSigma;
float magMeasurementSigma;
float airspeedMeasurementSigma;
float gyroProcessNoise;
float accelProcessNoise;
float EAS2TAS; // ratio f true to equivalent airspeed
void InitialiseParameters()
{
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
EAS2TAS = 1.0f;
yawVarScale = 1.0f;
windVelSigma = 0.1f;
dAngBiasSigma = 5.0e-7f;
dVelBiasSigma = 1e-4f;
magEarthSigma = 3.0e-4f;
magBodySigma = 3.0e-4f;
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
vneSigma = 0.2f;
vdSigma = 0.3f;
posNeSigma = 2.0f;
posDSigma = 2.0f;
magMeasurementSigma = 0.05;
airspeedMeasurementSigma = 1.4f;
gyroProcessNoise = 1.4544411e-2f;
accelProcessNoise = 0.5f;
}
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float posNED[3]; // North, East Down position (m)
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float innovVtas; // innovation output
float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s)
float magDeclination;
float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
// GPS input data variables
float gpsCourse;
float gpsVelD;
float gpsLat;
float gpsLon;
float gpsHgt;
uint8_t GPSstatus;
// Baro input
float baroHgt;
bool statesInitialised;
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused
bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection;
unsigned storeIndex;
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
void FuseVelposNED();
void FuseMagnetometer();
void FuseAirspeed();
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
void StoreStates(uint64_t timestamp_ms);
/**
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
*
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float statesForFusion[n_states], uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static float sq(float valIn);
void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float max);
void ConstrainVariances();
void ConstrainStates();
void ForceSymmetry();
int CheckAndBound();
void ResetPosition();
void ResetVelocity();
void ZeroVariables();
void GetFilterState(struct ekf_status_report *state);
void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN(struct ekf_status_report *err_report);
void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3], float declination);
protected:
bool FilterHealthy();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
};
uint32_t millis();
src/modules/ekf_att_pos_estimator/estimator.cpp → src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+292 -260
View File
@@ -1,143 +1,9 @@
#include "estimator.h"
#include "estimator_23states.h"
#include <string.h>
#include <stdio.h>
#include <stdarg.h>
// Define EKF_DEBUG here to enable the debug print calls
// if the macro is not set, these will be completely
// optimized out by the compiler.
//#define EKF_DEBUG
#ifdef EKF_DEBUG
#include <stdio.h>
static void
ekf_debug_print(const char *fmt, va_list args)
{
fprintf(stderr, "%s: ", "[ekf]");
vfprintf(stderr, fmt, args);
fprintf(stderr, "\n");
}
static void
ekf_debug(const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
ekf_debug_print(fmt, args);
}
#else
static void ekf_debug(const char *fmt, ...) { while(0){} }
#endif
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
}
void Vector3f::zero(void)
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
Mat3f::Mat3f() {
identity();
}
void Mat3f::identity() {
x.x = 1.0f;
x.y = 0.0f;
x.z = 0.0f;
y.x = 0.0f;
y.y = 1.0f;
y.z = 0.0f;
z.x = 0.0f;
z.y = 0.0f;
z.z = 1.0f;
}
Mat3f Mat3f::transpose(void) const
{
Mat3f ret = *this;
swap_var(ret.x.y, ret.y.x);
swap_var(ret.x.z, ret.z.x);
swap_var(ret.y.z, ret.z.y);
return ret;
}
// overload + operator to provide a vector addition
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.x + vecIn2.x;
vecOut.y = vecIn1.y + vecIn2.y;
vecOut.z = vecIn1.z + vecIn2.z;
return vecOut;
}
// overload - operator to provide a vector subtraction
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.x - vecIn2.x;
vecOut.y = vecIn1.y - vecIn2.y;
vecOut.z = vecIn1.z - vecIn2.z;
return vecOut;
}
// overload * operator to provide a matrix vector product
Vector3f operator*( Mat3f matIn, Vector3f vecIn)
{
Vector3f vecOut;
vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
return vecOut;
}
// overload % operator to provide a vector cross product
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
return vecOut;
}
// overload * operator to provide a vector scaler product
Vector3f operator*(Vector3f vecIn1, float sclIn1)
{
Vector3f vecOut;
vecOut.x = vecIn1.x * sclIn1;
vecOut.y = vecIn1.y * sclIn1;
vecOut.z = vecIn1.z * sclIn1;
return vecOut;
}
// overload * operator to provide a vector scaler product
Vector3f operator*(float sclIn1, Vector3f vecIn1)
{
Vector3f vecOut;
vecOut.x = vecIn1.x * sclIn1;
vecOut.y = vecIn1.y * sclIn1;
vecOut.z = vecIn1.z * sclIn1;
return vecOut;
}
void swap_var(float &d1, float &d2)
{
float tmp = d1;
d1 = d2;
d2 = tmp;
}
#define EKF_COVARIANCE_DIVERGED 1.0e8f
AttPosEKF::AttPosEKF()
@@ -145,7 +11,42 @@ AttPosEKF::AttPosEKF()
* instead to allow clean in-air re-initialization.
*/
{
summedDelAng.zero();
summedDelVel.zero();
fusionModeGPS = 0;
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
fuseMagData = false;
fuseVtasData = false;
onGround = true;
staticMode = true;
useAirspeed = true;
useCompass = true;
useRangeFinder = true;
numericalProtection = true;
refSet = false;
storeIndex = 0;
gpsHgt = 0.0f;
baroHgt = 0.0f;
GPSstatus = 0;
VtasMeas = 0.0f;
magDeclination = 0.0f;
dAngIMU.zero();
dVelIMU.zero();
velNED[0] = 0.0f;
velNED[1] = 0.0f;
velNED[2] = 0.0f;
accelGPSNED[0] = 0.0f;
accelGPSNED[1] = 0.0f;
accelGPSNED[2] = 0.0f;
delAngTotal.zero();
ekfDiverged = false;
lastReset = 0;
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
memset(&current_ekf_state, 0, sizeof(current_ekf_state));
ZeroVariables();
InitialiseParameters();
}
@@ -181,6 +82,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
dVelIMU.y = dVelIMU.y;
dVelIMU.z = dVelIMU.z - states[13];
delAngTotal.x += correctedDelAng.x;
delAngTotal.y += correctedDelAng.y;
delAngTotal.z += correctedDelAng.z;
// Save current measurements
Vector3f prevDelAng = correctedDelAng;
@@ -199,8 +104,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
}
else
{
deltaQuat[0] = cosf(0.5f*rotationMag);
float rotScaler = (sinf(0.5f*rotationMag))/rotationMag;
// We are using double here as we are unsure how small
// the angle differences are and if we get into numeric
// issues with float. The runtime impact is not measurable
// for these quantities.
deltaQuat[0] = cos(0.5*(double)rotationMag);
float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag;
deltaQuat[1] = correctedDelAng.x*rotScaler;
deltaQuat[2] = correctedDelAng.y*rotScaler;
deltaQuat[3] = correctedDelAng.z*rotScaler;
@@ -312,7 +221,8 @@ void AttPosEKF::CovariancePrediction(float dt)
float nextP[n_states][n_states];
// calculate covariance prediction process noise
for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f;
for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f;
for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f;
for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
// scale gyro bias noise when on ground to allow for faster bias estimation
for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
@@ -977,20 +887,20 @@ void AttPosEKF::CovariancePrediction(float dt)
// propagate
for (unsigned i = 0; i <= 13; i++) {
P[i][i] = nextP[i][i];
}
// force symmetry for observable states
// force zero for non-observable states
for (unsigned i = 1; i < n_states; i++)
// force symmetry for observable states
// force zero for non-observable states
for (unsigned i = 1; i < n_states; i++)
{
for (uint8_t j = 0; j < i; j++)
{
for (uint8_t j = 0; j < i; j++)
{
if ((i > 13) || (j > 13)) {
P[i][j] = 0.0f;
} else {
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
}
P[j][i] = P[i][j];
if ((i > 13) || (j > 13)) {
P[i][j] = 0.0f;
} else {
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
}
P[j][i] = P[i][j];
}
}
@@ -1020,9 +930,9 @@ void AttPosEKF::FuseVelposNED()
{
// declare variables used by fault isolation logic
uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
uint32_t hgtRetryTime = 5000; // height measurement retry time
uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure
uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available
uint32_t hgtRetryTime = 500; // height measurement retry time
uint32_t horizRetryTime;
// declare variables used to check measurement errors
@@ -1178,7 +1088,7 @@ void AttPosEKF::FuseVelposNED()
stateIndex = 4 + obsIndex;
// Calculate the measurement innovation, using states from a
// different time coordinate if fusing height data
if (obsIndex >= 0 && obsIndex <= 2)
if (obsIndex <= 2)
{
innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
}
@@ -1193,7 +1103,7 @@ void AttPosEKF::FuseVelposNED()
// Calculate the Kalman Gain
// Calculate innovation variances - also used for data logging
varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
SK = 1.0/varInnovVelPos[obsIndex];
SK = 1.0/(double)varInnovVelPos[obsIndex];
for (uint8_t i= 0; i<=indexLimit; i++)
{
Kfusion[i] = P[i][stateIndex]*SK;
@@ -1277,7 +1187,7 @@ void AttPosEKF::FuseMagnetometer()
// data fit is the only assumption we can make
// so we might as well take advantage of the computational efficiencies
// associated with sequential fusion
if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
if (useCompass && fuseMagData && (obsIndex < 3))
{
// Limit range of states modified when on ground
if(!onGround)
@@ -1293,7 +1203,7 @@ void AttPosEKF::FuseMagnetometer()
// three prediction time steps.
// Calculate observation jacobians and Kalman gains
if (fuseMagData)
if (obsIndex == 0)
{
// Copy required states to local variable names
q0 = statesAtMagMeasTime[0];
@@ -1388,11 +1298,6 @@ void AttPosEKF::FuseMagnetometer()
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
varInnovMag[0] = 1.0f/SK_MX[0];
innovMag[0] = MagPred[0] - magData.x;
// reset the observation index to 0 (we start by fusing the X
// measurement)
obsIndex = 0;
fuseMagData = false;
}
else if (obsIndex == 1) // we are now fusing the Y measurement
{
@@ -1508,7 +1413,7 @@ void AttPosEKF::FuseMagnetometer()
}
// Check the innovation for consistency and don't fuse if > 5Sigma
if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
{
// correct the state vector
for (uint8_t j= 0; j < indexLimit; j++)
@@ -1517,7 +1422,7 @@ void AttPosEKF::FuseMagnetometer()
}
// normalise the quaternion states
float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
if (quatMag > 1e-12)
if (quatMag > 1e-12f)
{
for (uint8_t j= 0; j<=3; j++)
{
@@ -1612,7 +1517,7 @@ void AttPosEKF::FuseAirspeed()
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
float H_TAS[n_states];
for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f;
H_TAS[4] = SH_TAS[2];
@@ -1661,7 +1566,7 @@ void AttPosEKF::FuseAirspeed()
// Calculate the measurement innovation
innovVtas = VtasPred - VtasMeas;
// Check the innovation for consistency and don't fuse if > 5Sigma
if ((innovVtas*innovVtas*SK_TAS) < 25.0)
if ((innovVtas*innovVtas*SK_TAS) < 25.0f)
{
// correct the state vector
for (uint8_t j=0; j <= 22; j++)
@@ -1758,7 +1663,7 @@ void AttPosEKF::FuseRangeFinder()
// Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
SH_RNG[4] = sin(rngFinderPitch);
cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch);
cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch);
if (useRangeFinder && cosRngTilt > 0.87f)
{
// Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset
@@ -1855,21 +1760,21 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
int64_t bestTimeDelta = 200;
unsigned bestStoreIndex = 0;
for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++)
{
// Work around a GCC compiler bug - we know 64bit support on ARM is
// sketchy in GCC.
uint64_t timeDelta;
if (msec > statetimeStamp[storeIndex]) {
timeDelta = msec - statetimeStamp[storeIndex];
if (msec > statetimeStamp[storeIndexLocal]) {
timeDelta = msec - statetimeStamp[storeIndexLocal];
} else {
timeDelta = statetimeStamp[storeIndex] - msec;
timeDelta = statetimeStamp[storeIndexLocal] - msec;
}
if (timeDelta < bestTimeDelta)
if (timeDelta < (uint64_t)bestTimeDelta)
{
bestStoreIndex = storeIndex;
bestStoreIndex = storeIndexLocal;
bestTimeDelta = timeDelta;
}
}
@@ -1926,7 +1831,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
Tnb.y.z = 2*(q23 + q01);
}
void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4])
{
// Calculate the body to nav cosine matrix
float q00 = sq(quat[0]);
@@ -1940,15 +1845,15 @@ void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
float q13 = quat[1]*quat[3];
float q23 = quat[2]*quat[3];
Tbn.x.x = q00 + q11 - q22 - q33;
Tbn.y.y = q00 - q11 + q22 - q33;
Tbn.z.z = q00 - q11 - q22 + q33;
Tbn.x.y = 2*(q12 - q03);
Tbn.x.z = 2*(q13 + q02);
Tbn.y.x = 2*(q12 + q03);
Tbn.y.z = 2*(q23 - q01);
Tbn.z.x = 2*(q13 - q02);
Tbn.z.y = 2*(q23 + q01);
Tbn_ret.x.x = q00 + q11 - q22 - q33;
Tbn_ret.y.y = q00 - q11 + q22 - q33;
Tbn_ret.z.z = q00 - q11 - q22 + q33;
Tbn_ret.x.y = 2*(q12 - q03);
Tbn_ret.x.z = 2*(q13 + q02);
Tbn_ret.y.x = 2*(q12 + q03);
Tbn_ret.y.z = 2*(q23 - q01);
Tbn_ret.z.x = 2*(q13 - q02);
Tbn_ret.z.y = 2*(q23 + q01);
}
void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
@@ -1979,17 +1884,17 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd,
velNED[2] = gpsVelD;
}
void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef)
void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
{
posNED[0] = earthRadius * (lat - latRef);
posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
posNED[2] = -(hgt - hgtRef);
posNED[0] = earthRadius * (lat - latReference);
posNED[1] = earthRadius * cos(latReference) * (lon - lonReference);
posNED[2] = -(hgt - hgtReference);
}
void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
{
lat = latRef + posNED[0] * earthRadiusInv;
lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
lat = latRef + (double)posNED[0] * earthRadiusInv;
lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
hgt = hgtRef - posNED[2];
}
@@ -2042,10 +1947,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max)
float ret;
if (val > max) {
ret = max;
ekf_debug("> max: %8.4f, val: %8.4f", max, val);
ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val);
} else if (val < min) {
ret = min;
ekf_debug("< min: %8.4f, val: %8.4f", min, val);
ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val);
} else {
ret = val;
}
@@ -2194,10 +2099,71 @@ void AttPosEKF::ForceSymmetry()
{
P[i][j] = 0.5f * (P[i][j] + P[j][i]);
P[j][i] = P[i][j];
if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) ||
(fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) {
current_ekf_state.covariancesExcessive = true;
current_ekf_state.error |= true;
InitializeDynamic(velNED, magDeclination);
return;
}
float symmetric = 0.5f * (P[i][j] + P[j][i]);
P[i][j] = symmetric;
P[j][i] = symmetric;
}
}
}
bool AttPosEKF::GyroOffsetsDiverged()
{
// Detect divergence by looking for rapid changes of the gyro offset
Vector3f current_bias;
current_bias.x = states[10];
current_bias.y = states[11];
current_bias.z = states[12];
Vector3f delta = current_bias - lastGyroOffset;
float delta_len = delta.length();
float delta_len_scaled = 0.0f;
// Protect against division by zero
if (delta_len > 0.0f) {
float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f);
delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU;
}
bool diverged = (delta_len_scaled > 1.0f);
lastGyroOffset = current_bias;
current_ekf_state.error |= diverged;
current_ekf_state.gyroOffsetsExcessive = diverged;
return diverged;
}
bool AttPosEKF::VelNEDDiverged()
{
Vector3f current_vel;
current_vel.x = states[4];
current_vel.y = states[5];
current_vel.z = states[6];
Vector3f gps_vel;
gps_vel.x = velNED[0];
gps_vel.y = velNED[1];
gps_vel.z = velNED[2];
Vector3f delta = current_vel - gps_vel;
float delta_len = delta.length();
bool excessive = (delta_len > 20.0f);
current_ekf_state.error |= excessive;
current_ekf_state.velOffsetExcessive = excessive;
return excessive;
}
bool AttPosEKF::FilterHealthy()
{
if (!statesInitialised) {
@@ -2262,42 +2228,26 @@ void AttPosEKF::ResetVelocity(void)
}
}
void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
{
for (unsigned i = 0; i < n_states; i++)
{
err->states[i] = states[i];
}
err->velHealth = current_ekf_state.velHealth;
err->posHealth = current_ekf_state.posHealth;
err->hgtHealth = current_ekf_state.hgtHealth;
err->velTimeout = current_ekf_state.velTimeout;
err->posTimeout = current_ekf_state.posTimeout;
err->hgtTimeout = current_ekf_state.hgtTimeout;
}
bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
bool AttPosEKF::StatesNaN() {
bool err = false;
// check all integrators
if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) {
err_report->statesNaN = true;
current_ekf_state.angNaN = true;
ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
err = true;
goto out;
} // delta angles
if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) {
err_report->statesNaN = true;
current_ekf_state.angNaN = true;
ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
err = true;
goto out;
} // delta angles
if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) {
err_report->statesNaN = true;
current_ekf_state.summedDelVelNaN = true;
ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
err = true;
goto out;
@@ -2308,7 +2258,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
for (unsigned j = 0; j < n_states; j++) {
if (!isfinite(KH[i][j])) {
err_report->covarianceNaN = true;
current_ekf_state.KHNaN = true;
err = true;
ekf_debug("KH NaN");
goto out;
@@ -2316,7 +2266,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
if (!isfinite(KHP[i][j])) {
err_report->covarianceNaN = true;
current_ekf_state.KHPNaN = true;
err = true;
ekf_debug("KHP NaN");
goto out;
@@ -2324,7 +2274,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
if (!isfinite(P[i][j])) {
err_report->covarianceNaN = true;
current_ekf_state.covarianceNaN = true;
err = true;
ekf_debug("P NaN");
} // covariance matrix
@@ -2332,7 +2282,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
if (!isfinite(Kfusion[i])) {
err_report->kalmanGainsNaN = true;
current_ekf_state.kalmanGainsNaN = true;
ekf_debug("Kfusion NaN");
err = true;
goto out;
@@ -2340,7 +2290,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
if (!isfinite(states[i])) {
err_report->statesNaN = true;
current_ekf_state.statesNaN = true;
ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);
err = true;
goto out;
@@ -2349,7 +2299,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
out:
if (err) {
FillErrorReport(err_report);
current_ekf_state.error |= true;
}
return err;
@@ -2365,47 +2315,105 @@ out:
* updated, but before any of the fusion steps are
* executed.
*/
int AttPosEKF::CheckAndBound()
int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
{
// Store the old filter state
bool currStaticMode = staticMode;
// Limit reset rate to 5 Hz to allow the filter
// to settle
if (millis() - lastReset < 200) {
return 0;
}
if (ekfDiverged) {
ekfDiverged = false;
}
int ret = 0;
// Check if we're on ground - this also sets static mode.
OnGroundCheck();
// Reset the filter if the states went NaN
if (StatesNaN(&last_ekf_error)) {
if (StatesNaN()) {
ekf_debug("re-initializing dynamic");
InitializeDynamic(velNED, magDeclination);
// Reset and fill error report
InitializeDynamic(velNED, magDeclination);
return 1;
ret = 1;
}
// Reset the filter if the IMU data is too old
if (dtIMU > 0.3f) {
current_ekf_state.imuTimeout = true;
// Fill error report
GetFilterState(&last_ekf_error);
ResetVelocity();
ResetPosition();
ResetHeight();
ResetStoredStates();
// that's all we can do here, return
return 2;
}
// Timeout cleared with this reset
current_ekf_state.imuTimeout = false;
// Check if we're on ground - this also sets static mode.
OnGroundCheck();
// that's all we can do here, return
ret = 2;
}
// Check if we switched between states
if (currStaticMode != staticMode) {
// Fill error report, but not setting error flag
GetFilterState(&last_ekf_error);
ResetVelocity();
ResetPosition();
ResetHeight();
ResetStoredStates();
return 3;
ret = 3;
}
return 0;
// Reset the filter if gyro offsets are excessive
if (GyroOffsetsDiverged()) {
// Reset and fill error report
InitializeDynamic(velNED, magDeclination);
// that's all we can do here, return
ret = 4;
}
// Reset the filter if it diverges too far from GPS
if (VelNEDDiverged()) {
// Reset and fill error report
InitializeDynamic(velNED, magDeclination);
// that's all we can do here, return
ret = 5;
}
// The excessive covariance detection already
// reset the filter. Just need to report here.
if (last_ekf_error.covariancesExcessive) {
ret = 6;
}
if (ret) {
ekfDiverged = true;
lastReset = millis();
// This reads the last error and clears it
GetLastErrorState(last_error);
}
return ret;
}
void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
@@ -2456,6 +2464,30 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
{
if (current_ekf_state.error) {
GetFilterState(&last_ekf_error);
}
ZeroVariables();
// Reset error states
current_ekf_state.error = false;
current_ekf_state.angNaN = false;
current_ekf_state.summedDelVelNaN = false;
current_ekf_state.KHNaN = false;
current_ekf_state.KHPNaN = false;
current_ekf_state.PNaN = false;
current_ekf_state.covarianceNaN = false;
current_ekf_state.kalmanGainsNaN = false;
current_ekf_state.statesNaN = false;
current_ekf_state.velHealth = true;
//current_ekf_state.posHealth = ?;
//current_ekf_state.hgtHealth = ?;
current_ekf_state.velTimeout = false;
//current_ekf_state.posTimeout = ?;
//current_ekf_state.hgtTimeout = ?;
// Fill variables with valid data
velNED[0] = initvelNED[0];
@@ -2494,7 +2526,11 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
// write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel
// positions:
states[7] = posNE[0];
states[8] = posNE[1];
states[9] = -hgtMea;
for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
states[16] = initMagNED.x; // Magnetic Field North
states[17] = initMagNED.y; // Magnetic Field East
states[18] = initMagNED.z; // Magnetic Field Down
@@ -2525,14 +2561,16 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
hgtRef = referenceHgt;
refSet = true;
// we are at reference altitude, so measurement must be zero
hgtMea = 0.0f;
// we are at reference position, so measurement must be zero
posNE[0] = 0.0f;
posNE[1] = 0.0f;
// we are at an unknown, possibly non-zero altitude - so altitude
// is not reset (hgtMea)
// the baro offset must be this difference now
baroHgtOffset = baroHgt - referenceHgt;
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
InitializeDynamic(initvelNED, declination);
}
@@ -2540,27 +2578,8 @@ void AttPosEKF::ZeroVariables()
{
// Initialize on-init initialized variables
fusionModeGPS = 0;
covSkipCount = 0;
statesInitialised = false;
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
fuseMagData = false;
fuseVtasData = false;
onGround = true;
staticMode = true;
useAirspeed = true;
useCompass = true;
useRangeFinder = true;
numericalProtection = true;
refSet = false;
storeIndex = 0;
gpsHgt = 0.0f;
baroHgt = 0.0f;
GPSstatus = 0;
VtasMeas = 0.0f;
magDeclination = 0.0f;
// Do the data structure init
for (unsigned i = 0; i < n_states; i++) {
@@ -2577,9 +2596,7 @@ void AttPosEKF::ZeroVariables()
correctedDelAng.zero();
summedDelAng.zero();
summedDelVel.zero();
dAngIMU.zero();
dVelIMU.zero();
lastGyroOffset.zero();
for (unsigned i = 0; i < data_buffer_size; i++) {
@@ -2598,12 +2615,27 @@ void AttPosEKF::ZeroVariables()
}
void AttPosEKF::GetFilterState(struct ekf_status_report *state)
void AttPosEKF::GetFilterState(struct ekf_status_report *err)
{
memcpy(state, &current_ekf_state, sizeof(*state));
// Copy states
for (unsigned i = 0; i < n_states; i++) {
current_ekf_state.states[i] = states[i];
}
current_ekf_state.n_states = n_states;
memcpy(err, &current_ekf_state, sizeof(*err));
// err->velHealth = current_ekf_state.velHealth;
// err->posHealth = current_ekf_state.posHealth;
// err->hgtHealth = current_ekf_state.hgtHealth;
// err->velTimeout = current_ekf_state.velTimeout;
// err->posTimeout = current_ekf_state.posTimeout;
// err->hgtTimeout = current_ekf_state.hgtTimeout;
}
void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
{
memcpy(last_error, &last_ekf_error, sizeof(*last_error));
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
}
src/modules/ekf_att_pos_estimator/estimator.h → src/modules/ekf_att_pos_estimator/estimator_23states.h
+24 -76
View File
@@ -1,76 +1,10 @@
#include <math.h>
#include <stdint.h>
#pragma once
#define GRAVITY_MSS 9.80665f
#define deg2rad 0.017453292f
#define rad2deg 57.295780f
#define pi 3.141592657f
#define earthRate 0.000072921f
#define earthRadius 6378145.0f
#define earthRadiusInv 1.5678540e-7f
class Vector3f
{
private:
public:
float x;
float y;
float z;
float length(void) const;
void zero(void);
};
class Mat3f
{
private:
public:
Vector3f x;
Vector3f y;
Vector3f z;
Mat3f();
void identity();
Mat3f transpose(void) const;
};
Vector3f operator*(float sclIn1, Vector3f vecIn1);
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*( Mat3f matIn, Vector3f vecIn);
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*(Vector3f vecIn1, float sclIn1);
void swap_var(float &d1, float &d2);
#include "estimator_utilities.h"
const unsigned int n_states = 23;
const unsigned int data_buffer_size = 50;
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2,
GPS_FIX_3D = 3
};
struct ekf_status_report {
bool velHealth;
bool posHealth;
bool hgtHealth;
bool velTimeout;
bool posTimeout;
bool hgtTimeout;
uint32_t velFailTime;
uint32_t posFailTime;
uint32_t hgtFailTime;
float states[n_states];
bool statesNaN;
bool covarianceNaN;
bool kalmanGainsNaN;
};
class AttPosEKF {
public:
@@ -141,7 +75,7 @@ public:
accelProcessNoise = 0.5f;
}
struct {
struct mag_state_struct {
unsigned obsIndex;
float MagPred[3];
float SH_MAG[9];
@@ -157,7 +91,12 @@ public:
float magZbias;
float R_MAG;
Mat3f DCM;
} magstate;
};
struct mag_state_struct magstate;
struct mag_state_struct resetMagState;
// Global variables
@@ -166,6 +105,7 @@ public:
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
float resetStates[n_states];
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
@@ -183,6 +123,8 @@ public:
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f lastGyroOffset; // Last gyro offset
Vector3f delAngTotal;
Mat3f Tbn; // transformation matrix from body to NED coordinates
Mat3f Tnb; // transformation amtrix from NED to body coordinates
@@ -196,11 +138,11 @@ public:
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
float rngMea; // Ground distance
float posNED[3]; // North, East Down position (m)
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
@@ -243,6 +185,9 @@ public:
bool useCompass; ///< boolean true if magnetometer data is being used
bool useRangeFinder; ///< true when rangefinder is being used
bool ekfDiverged;
uint64_t lastReset;
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
@@ -289,7 +234,7 @@ int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
@@ -299,9 +244,9 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
@@ -321,7 +266,7 @@ void ConstrainStates();
void ForceSymmetry();
int CheckAndBound();
int CheckAndBound(struct ekf_status_report *last_error);
void ResetPosition();
@@ -333,8 +278,7 @@ void GetFilterState(struct ekf_status_report *state);
void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN(struct ekf_status_report *err_report);
void FillErrorReport(struct ekf_status_report *err);
bool StatesNaN();
void InitializeDynamic(float (&initvelNED)[3], float declination);
@@ -342,6 +286,10 @@ protected:
bool FilterHealthy();
bool GyroOffsetsDiverged();
bool VelNEDDiverged();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+139
View File
@@ -0,0 +1,139 @@
#include "estimator_utilities.h"
// Define EKF_DEBUG here to enable the debug print calls
// if the macro is not set, these will be completely
// optimized out by the compiler.
//#define EKF_DEBUG
#ifdef EKF_DEBUG
#include <stdio.h>
static void
ekf_debug_print(const char *fmt, va_list args)
{
fprintf(stderr, "%s: ", "[ekf]");
vfprintf(stderr, fmt, args);
fprintf(stderr, "\n");
}
void
ekf_debug(const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
ekf_debug_print(fmt, args);
}
#else
void ekf_debug(const char *fmt, ...) { while(0){} }
#endif
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
}
void Vector3f::zero(void)
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
Mat3f::Mat3f() {
identity();
}
void Mat3f::identity() {
x.x = 1.0f;
x.y = 0.0f;
x.z = 0.0f;
y.x = 0.0f;
y.y = 1.0f;
y.z = 0.0f;
z.x = 0.0f;
z.y = 0.0f;
z.z = 1.0f;
}
Mat3f Mat3f::transpose(void) const
{
Mat3f ret = *this;
swap_var(ret.x.y, ret.y.x);
swap_var(ret.x.z, ret.z.x);
swap_var(ret.y.z, ret.z.y);
return ret;
}
// overload + operator to provide a vector addition
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.x + vecIn2.x;
vecOut.y = vecIn1.y + vecIn2.y;
vecOut.z = vecIn1.z + vecIn2.z;
return vecOut;
}
// overload - operator to provide a vector subtraction
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.x - vecIn2.x;
vecOut.y = vecIn1.y - vecIn2.y;
vecOut.z = vecIn1.z - vecIn2.z;
return vecOut;
}
// overload * operator to provide a matrix vector product
Vector3f operator*( Mat3f matIn, Vector3f vecIn)
{
Vector3f vecOut;
vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
return vecOut;
}
// overload % operator to provide a vector cross product
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
return vecOut;
}
// overload * operator to provide a vector scaler product
Vector3f operator*(Vector3f vecIn1, float sclIn1)
{
Vector3f vecOut;
vecOut.x = vecIn1.x * sclIn1;
vecOut.y = vecIn1.y * sclIn1;
vecOut.z = vecIn1.z * sclIn1;
return vecOut;
}
// overload * operator to provide a vector scaler product
Vector3f operator*(float sclIn1, Vector3f vecIn1)
{
Vector3f vecOut;
vecOut.x = vecIn1.x * sclIn1;
vecOut.y = vecIn1.y * sclIn1;
vecOut.z = vecIn1.z * sclIn1;
return vecOut;
}
void swap_var(float &d1, float &d2)
{
float tmp = d1;
d1 = d2;
d2 = tmp;
}
src/modules/ekf_att_pos_estimator/estimator_utilities.h
+82
View File
@@ -0,0 +1,82 @@
#include <math.h>
#include <stdint.h>
#pragma once
#define GRAVITY_MSS 9.80665f
#define deg2rad 0.017453292f
#define rad2deg 57.295780f
#define pi 3.141592657f
#define earthRate 0.000072921f
#define earthRadius 6378145.0
#define earthRadiusInv 1.5678540e-7
class Vector3f
{
private:
public:
float x;
float y;
float z;
float length(void) const;
void zero(void);
};
class Mat3f
{
private:
public:
Vector3f x;
Vector3f y;
Vector3f z;
Mat3f();
void identity();
Mat3f transpose(void) const;
};
Vector3f operator*(float sclIn1, Vector3f vecIn1);
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*( Mat3f matIn, Vector3f vecIn);
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*(Vector3f vecIn1, float sclIn1);
void swap_var(float &d1, float &d2);
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2,
GPS_FIX_3D = 3
};
struct ekf_status_report {
bool error;
bool velHealth;
bool posHealth;
bool hgtHealth;
bool velTimeout;
bool posTimeout;
bool hgtTimeout;
bool imuTimeout;
uint32_t velFailTime;
uint32_t posFailTime;
uint32_t hgtFailTime;
float states[32];
unsigned n_states;
bool angNaN;
bool summedDelVelNaN;
bool KHNaN;
bool KHPNaN;
bool PNaN;
bool covarianceNaN;
bool kalmanGainsNaN;
bool statesNaN;
bool gyroOffsetsExcessive;
bool covariancesExcessive;
bool velOffsetExcessive;
};
void ekf_debug(const char *fmt, ...);
src/modules/ekf_att_pos_estimator/module.mk
+2 -1
View File
@@ -39,4 +39,5 @@ MODULE_COMMAND = ekf_att_pos_estimator
SRCS = ekf_att_pos_estimator_main.cpp \
ekf_att_pos_estimator_params.c \
estimator.cpp
estimator_23states.cpp \
estimator_utilities.cpp
src/modules/fw_att_control/fw_att_control_main.cpp
+9 -2
View File
@@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main()
float throttle_sp = 0.0f;
if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral)
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else {
/*
* Scale down roll and pitch as the setpoints are radians
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+178 -116
View File
@@ -43,8 +43,8 @@
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
*
* Original implementation for total energy control class:
* Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
* Implementation for total energy control class:
* Thomas Gubler
*
* More details and acknowledgements in the referenced library headers.
*
@@ -88,9 +88,9 @@
#include <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
/**
@@ -153,8 +153,6 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
/** manual control states */
float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */
double _loiter_hold_lat;
@@ -200,7 +198,8 @@ private:
math::Matrix<3, 3> _R_nb; ///< current attitude
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
struct {
float l1_period;
@@ -343,11 +342,11 @@ private:
/**
* Control position.
*/
bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &_pos_sp_triplet);
float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);
/**
* Shim for calling task_main from task_create.
@@ -368,6 +367,19 @@ private:
* Reset landing state
*/
void reset_landing_state();
/*
* Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
* XXX need to clean up/remove this function once mtecs fully replaces TECS
*/
void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
tecs_mode mode = TECS_MODE_NORMAL);
};
namespace l1_control
@@ -406,7 +418,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
/* states */
_setpoint_valid(false),
_loiter_hold(false),
land_noreturn_horizontal(false),
land_noreturn_vertical(false),
@@ -431,6 +442,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_global_pos(),
_pos_sp_triplet(),
_sensor_combined(),
_mTecs(),
_was_pos_control_mode(false),
_range_finder()
{
_nav_capabilities.turn_distance = 0.0f;
@@ -550,23 +563,6 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
_tecs.set_max_sink_rate(_parameters.max_sink_rate);
_tecs.set_throttle_damp(_parameters.throttle_damp);
_tecs.set_integrator_gain(_parameters.integrator_gain);
_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
_tecs.set_speed_weight(_parameters.speed_weight);
_tecs.set_pitch_damping(_parameters.pitch_damping);
_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
_tecs.set_heightrate_p(_parameters.heightrate_p);
_tecs.set_speedrate_p(_parameters.speedrate_p);
/* sanity check parameters */
if (_parameters.airspeed_max < _parameters.airspeed_min ||
_parameters.airspeed_max < 5.0f ||
@@ -589,6 +585,9 @@ FixedwingPositionControl::parameters_update()
/* Update Launch Detector Parameters */
launchDetector.updateParams();
/* Update the mTecs */
_mTecs.updateParams();
return OK;
}
@@ -635,9 +634,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
@@ -692,7 +688,6 @@ FixedwingPositionControl::vehicle_setpoint_poll()
if (pos_sp_triplet_updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
_setpoint_valid = true;
}
}
@@ -734,15 +729,15 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
}
void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
yaw_vector.normalize();
float ground_speed_body = yaw_vector * ground_speed;
float ground_speed_body = yaw_vector * ground_speed_2d;
/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
float distance = 0.0f;
@@ -801,12 +796,13 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt,
}
bool
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
{
bool setpoint = true;
calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
@@ -817,7 +813,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
@@ -827,20 +822,31 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// XXX this should only execute if auto AND safety off (actuators active),
// else integrators should be constantly reset.
if (_control_mode.flag_control_position_enabled) {
if (pos_sp_triplet.current.valid) {
if (!_was_pos_control_mode) {
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}
_was_pos_control_mode = true;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
/* current waypoint (the one currently heading for) */
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* current waypoint (the one currently heading for) */
math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* Initialize attitude controller integrator reset flags to 0 */
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
/* previous waypoint */
math::Vector<2> prev_wp;
@@ -859,31 +865,29 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
/* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
pos_sp_triplet.current.loiter_direction, ground_speed);
pos_sp_triplet.current.loiter_direction, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
@@ -908,7 +912,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
@@ -918,7 +922,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
/* normal navigation */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
}
_att_sp.roll_body = _l1_control.nav_roll();
@@ -942,7 +946,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance);
float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
@@ -977,11 +981,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
land_stayonground = true;
}
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_pitch_angle_rad,
0.0f, throttle_max, throttle_land,
flare_pitch_angle_rad, math::radians(15.0f));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land), eas2tas,
flare_pitch_angle_rad, math::radians(15.0f),
0.0f, throttle_max, throttle_land,
false, flare_pitch_angle_rad,
_pos_sp_triplet.current.alt + relative_alt, ground_speed,
land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
if (!land_noreturn_vertical) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -994,11 +1000,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* intersect glide slope:
* minimize speed to approach speed
* if current position is higher or within 10m of slope follow the glide slope
* if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
* if current position is higher than the slope follow the glide slope (sink to the
* glide slope)
* also if the system captures the slope it should stay
* on the slope (bool land_onslope)
* if current position is below the slope continue at previous wp altitude
* until the intersection with slope
* */
float altitude_desired_rel = relative_alt;
if (relative_alt > landing_slope_alt_rel_desired - 10.0f) {
if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
@@ -1007,28 +1017,40 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
} else {
/* continue horizontally */
altitude_desired_rel = math::max(relative_alt, L_altitude_rel);
altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
}
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel,
calculate_target_airspeed(airspeed_approach), eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_pos_sp_triplet.current.alt + relative_alt,
ground_speed);
}
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
// warnx("Launch detection running");
if(!launch_detected) { //do not do further checks once a launch was detected
if (launchDetector.launchDetectionEnabled()) {
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
// warnx("Launch detection running");
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
last_sent = hrt_absolute_time();
}
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
if (launchDetector.getLaunchDetected()) {
launch_detected = true;
@@ -1041,7 +1063,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
@@ -1052,22 +1074,36 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (altitude_error > 15.0f) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max,
_parameters.throttle_cruise,
true,
math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f)),
_global_pos.alt,
ground_speed,
TECS_MODE_TAKEOFF);
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
} else {
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
calculate_target_airspeed(_parameters.airspeed_trim),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
}
} else {
@@ -1101,19 +1137,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (0/* posctrl mode enabled */) {
_was_pos_control_mode = false;
/** POSCTRL FLIGHT **/
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
//XXX not used
//XXX not used
/* climb out control */
/* climb out control */
// bool climb_out = false;
//
// /* user wants to climb out */
@@ -1121,25 +1159,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// climb_out = true;
// }
/* if in altctrl mode, set airspeed based on manual control */
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (0/* altctrl mode enabled */) {
_was_pos_control_mode = false;
/** ALTCTRL FLIGHT **/
if (0/* switched from another mode to altctrl */) {
@@ -1160,8 +1199,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* user switched off throttle */
if (_manual.z < 0.1f) {
throttle_max = 0.0f;
/* switch to pure pitch based altitude control, give up speed */
_tecs.set_speed_weight(0.0f);
}
/* climb out control */
@@ -1172,18 +1209,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
climb_out = true;
}
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _manual.y;
_att_sp.yaw_body = _manual.r;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
climb_out, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
climb_out, math::radians(_parameters.pitch_limit_min),
_global_pos.alt, ground_speed);
} else {
_was_pos_control_mode = false;
/** MANUAL FLIGHT **/
/* no flight mode applies, do not publish an attitude setpoint */
@@ -1200,9 +1238,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
_att_sp.pitch_body = _mTecs.getPitchSetpoint();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@@ -1316,7 +1354,7 @@ FixedwingPositionControl::task_main()
range_finder_poll();
// vehicle_baro_poll();
math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
/*
@@ -1378,6 +1416,30 @@ void FixedwingPositionControl::reset_landing_state()
land_onslope = false;
}
void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
tecs_mode mode)
{
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
}
int
FixedwingPositionControl::start()
{
src/modules/fw_pos_control_l1/landingslope.cpp
+8 -8
View File
@@ -46,16 +46,16 @@
#include <unistd.h>
#include <mathlib/mathlib.h>
void Landingslope::update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_relative_alt,
float H1_virt)
void Landingslope::update(float landing_slope_angle_rad_new,
float flare_relative_alt_new,
float motor_lim_relative_alt_new,
float H1_virt_new)
{
_landing_slope_angle_rad = landing_slope_angle_rad;
_flare_relative_alt = flare_relative_alt;
_motor_lim_relative_alt = motor_lim_relative_alt;
_H1_virt = H1_virt;
_landing_slope_angle_rad = landing_slope_angle_rad_new;
_flare_relative_alt = flare_relative_alt_new;
_motor_lim_relative_alt = motor_lim_relative_alt_new;
_H1_virt = H1_virt_new;
calculateSlopeValues();
}
src/modules/fw_pos_control_l1/landingslope.h
+4 -4
View File
@@ -123,10 +123,10 @@ public:
float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
void update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_relative_alt,
float H1_virt);
void update(float landing_slope_angle_rad_new,
float flare_relative_alt_new,
float motor_lim_relative_alt_new,
float H1_virt_new);
inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
src/modules/fw_pos_control_l1/module.mk
+4 -1
View File
@@ -39,4 +39,7 @@ MODULE_COMMAND = fw_pos_control_l1
SRCS = fw_pos_control_l1_main.cpp \
fw_pos_control_l1_params.c \
landingslope.cpp
landingslope.cpp \
mtecs/mTecs.cpp \
mtecs/limitoverride.cpp \
mtecs/mTecs_params.c
src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
+71
View File
@@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@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 limitoverride.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "limitoverride.h"
namespace fwPosctrl {
bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch)
{
bool ret = false;
if (overrideThrottleMinEnabled) {
outputLimiterThrottle.setMin(overrideThrottleMin);
ret = true;
}
if (overrideThrottleMaxEnabled) {
outputLimiterThrottle.setMax(overrideThrottleMax);
ret = true;
}
if (overridePitchMinEnabled) {
outputLimiterPitch.setMin(overridePitchMin);
ret = true;
}
if (overridePitchMaxEnabled) {
outputLimiterPitch.setMax(overridePitchMax);
ret = true;
}
return ret;
}
} /* namespace fwPosctrl */
src/modules/fw_pos_control_l1/mtecs/limitoverride.h
+107
View File
@@ -0,0 +1,107 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@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 limitoverride.h
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef LIMITOVERRIDE_H_
#define LIMITOVERRIDE_H_
#include "mTecs_blocks.h"
namespace fwPosctrl
{
/* A small class which provides helper functions to override control output limits which are usually set by
* parameters in special cases
*/
class LimitOverride
{
public:
LimitOverride() :
overrideThrottleMinEnabled(false),
overrideThrottleMaxEnabled(false),
overridePitchMinEnabled(false),
overridePitchMaxEnabled(false)
{};
~LimitOverride() {};
/*
* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
* this class (if enabled)
* @return true if the limit was applied
*/
bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch);
/* Functions to enable or disable the override */
void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
&overrideThrottleMin, value); }
void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
&overrideThrottleMax, value); }
void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
&overridePitchMin, value); }
void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
&overridePitchMax, value); }
void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
protected:
bool overrideThrottleMinEnabled;
float overrideThrottleMin;
bool overrideThrottleMaxEnabled;
float overrideThrottleMax;
bool overridePitchMinEnabled;
float overridePitchMin; //in degrees (replaces param values)
bool overridePitchMaxEnabled;
float overridePitchMax; //in degrees (replaces param values)
/* Enable a specific limit override */
void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; };
/* Disable a specific limit override */
void disable(bool *flag) { *flag = false; };
};
} /* namespace fwPosctrl */
#endif /* LIMITOVERRIDE_H_ */
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+313
View File
@@ -0,0 +1,313 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@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 mTecs.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "mTecs.h"
#include <lib/geo/geo.h>
#include <stdio.h>
namespace fwPosctrl {
mTecs::mTecs() :
SuperBlock(NULL, "MT"),
/* Parameters */
_mTecsEnabled(this, "ENABLED"),
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
_status(&getPublications(), ORB_ID(tecs_status)),
/* control blocks */
_controlTotalEnergy(this, "THR"),
_controlEnergyDistribution(this, "PIT", true),
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
_pitchSp(0.0f),
_BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"),
_BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
_BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"),
_BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true),
_BlockOutputLimiterLandThrottle(this, "LND_THR"),
_BlockOutputLimiterLandPitch(this, "LND_PIT", true),
timestampLastIteration(hrt_absolute_time()),
_firstIterationAfterReset(true),
_dtCalculated(false),
_counter(0),
_debug(false)
{
}
mTecs::~mTecs()
{
}
int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
!isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
return -1;
}
/* time measurement */
updateTimeMeasurement();
/* calculate flight path angle setpoint from altitude setpoint */
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
/* Debug output */
if (_counter % 10 == 0) {
debug("***");
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
}
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
/* use flightpath angle setpoint for total energy control */
return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
airspeedSp, mode, limitOverride);
}
int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
!isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
return -1;
}
/* time measurement */
updateTimeMeasurement();
/* Filter airspeed */
float airspeedFiltered = _airspeedLowpass.update(airspeed);
/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered);
/* Debug output */
if (_counter % 10 == 0) {
debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f,"
"accelerationLongitudinalSp%.4f",
(double)airspeedSp, (double)airspeed,
(double)airspeedFiltered, (double)accelerationLongitudinalSp);
}
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
_status.airspeed = airspeed;
_status.airspeedFiltered = airspeedFiltered;
/* use longitudinal acceleration setpoint for total energy control */
return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered,
accelerationLongitudinalSp, mode, limitOverride);
}
int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
!isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
return -1;
}
/* time measurement */
updateTimeMeasurement();
/* update parameters first */
updateParams();
/* Filter flightpathangle */
float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
/* calculate values (energies) */
float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
float airspeedDerivative = 0.0f;
if(_airspeedDerivative.getDt() > 0.0f) {
airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
}
float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
float airspeedDerivativeSp = accelerationLongitudinalSp;
float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
/* Debug output */
if (_counter % 10 == 0) {
debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f",
(double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm);
debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f",
(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
}
/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
mode = TECS_MODE_UNDERSPEED;
}
/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == TECS_MODE_TAKEOFF) {
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
} else if (mode == TECS_MODE_LAND) {
// only limit pitch but do not limit throttle
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == TECS_MODE_LAND_THROTTLELIM) {
outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == TECS_MODE_UNDERSPEED) {
outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
}
/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
* is running) */
bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.flightPathAngleFiltered = flightPathAngleFiltered;
_status.airspeedDerivativeSp = airspeedDerivativeSp;
_status.airspeedDerivative = airspeedDerivative;
_status.totalEnergyRateSp = totalEnergyRateSp;
_status.totalEnergyRate = totalEnergyRate;
_status.energyDistributionRateSp = energyDistributionRateSp;
_status.energyDistributionRate = energyDistributionRate;
_status.mode = mode;
/** update control blocks **/
/* update total energy rate control block */
_throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle);
/* update energy distribution rate control block */
_pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch);
if (_counter % 10 == 0) {
debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f",
(double)_throttleSp, (double)_pitchSp,
(double)flightPathAngleSp, (double)flightPathAngle,
(double)accelerationLongitudinalSp, (double)airspeedDerivative);
}
/* publish status messge */
_status.update();
/* clean up */
_firstIterationAfterReset = false;
_dtCalculated = false;
_counter++;
return 0;
}
void mTecs::resetIntegrators()
{
_controlTotalEnergy.getIntegral().setY(0.0f);
_controlEnergyDistribution.getIntegral().setY(0.0f);
timestampLastIteration = hrt_absolute_time();
_firstIterationAfterReset = true;
}
void mTecs::resetDerivatives(float airspeed)
{
_airspeedDerivative.setU(airspeed);
}
void mTecs::updateTimeMeasurement()
{
if (!_dtCalculated) {
float deltaTSeconds = 0.0f;
if (!_firstIterationAfterReset) {
hrt_abstime timestampNow = hrt_absolute_time();
deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
timestampLastIteration = timestampNow;
}
setDt(deltaTSeconds);
_dtCalculated = true;
}
}
void mTecs::debug_print(const char *fmt, va_list args)
{
fprintf(stderr, "%s: ", "[mtecs]");
vfprintf(stderr, fmt, args);
fprintf(stderr, "\n");
}
void mTecs::debug(const char *fmt, ...) {
if (!_debug) {
return;
}
va_list args;
va_start(args, fmt);
debug_print(fmt, args);
}
} /* namespace fwPosctrl */
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+155
View File
@@ -0,0 +1,155 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@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 mTecs.h
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef MTECS_H_
#define MTECS_H_
#include "mTecs_blocks.h"
#include "limitoverride.h"
#include <controllib/block/BlockParam.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/tecs_status.h>
namespace fwPosctrl
{
/* Main class of the mTecs */
class mTecs : public control::SuperBlock
{
public:
mTecs();
virtual ~mTecs();
/*
* Control in altitude setpoint and speed mode
*/
int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
/*
* Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
*/
int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
/*
* Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
*/
int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
/*
* Reset all integrators
*/
void resetIntegrators();
/*
* Reset all derivative calculations
*/
void resetDerivatives(float airspeed);
/* Accessors */
bool getEnabled() { return _mTecsEnabled.get() > 0; }
float getThrottleSetpoint() { return _throttleSp; }
float getPitchSetpoint() { return _pitchSp; }
float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
protected:
/* parameters */
control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */
control::BlockParamFloat _airspeedMin; /**< minimal airspeed */
/* Publications */
uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */
/* control blocks */
BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output
is throttle */
BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control:
output is pitch */
BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight
path angle setpoint */
BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration
setpoint */
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
/* Output setpoints */
float _throttleSp; /**< Throttle Setpoint from 0 to 1 */
float _pitchSp; /**< Pitch Setpoint from -pi to pi */
/* Output Limits in special modes */
BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */
BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */
BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in
last phase)*/
BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */
/* Time measurements */
hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */
bool _firstIterationAfterReset; /**< True during the first iteration after a reset */
bool _dtCalculated; /**< True if dt has been calculated in this iteration */
int _counter;
bool _debug; ///< Set true to enable debug output
static void debug_print(const char *fmt, va_list args);
void debug(const char *fmt, ...);
/*
* Measure and update the time step dt if this was not already done in the current iteration
*/
void updateTimeMeasurement();
};
} /* namespace fwPosctrl */
#endif /* MTECS_H_ */
src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+220
View File
@@ -0,0 +1,220 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@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 mTecs_blocks.h
*
* Custom blocks for the mTecs
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include <controllib/blocks.hpp>
#include <systemlib/err.h>
namespace fwPosctrl
{
using namespace control;
/* An block which can be used to limit the output */
class BlockOutputLimiter: public SuperBlock
{
public:
// methods
BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
SuperBlock(parent, name),
_isAngularLimit(isAngularLimit),
_min(this, "MIN"),
_max(this, "MAX")
{};
virtual ~BlockOutputLimiter() {};
/*
* Imposes the limits given by _min and _max on value
*
* @param value is changed to be on the interval _min to _max
* @param difference if the value is changed this corresponds to the change of value * (-1)
* otherwise unchanged
* @return: true if the limit is applied, false otherwise
*/
bool limit(float& value, float& difference) {
float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
if (value < minimum) {
difference = value - minimum;
value = minimum;
return true;
} else if (value > maximum) {
difference = value - maximum;
value = maximum;
return true;
}
return false;
}
//accessor:
bool isAngularLimit() {return _isAngularLimit ;}
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
void setMin(float value) { _min.set(value); }
void setMax(float value) { _max.set(value); }
protected:
//attributes
bool _isAngularLimit;
control::BlockParamFloat _min;
control::BlockParamFloat _max;
};
/* A combination of feed forward, P and I gain using the output limiter*/
class BlockFFPILimited: public SuperBlock
{
public:
// methods
BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
SuperBlock(parent, name),
_outputLimiter(this, "", isAngularLimit),
_integral(this, "I"),
_kFF(this, "FF"),
_kP(this, "P"),
_kI(this, "I"),
_offset(this, "OFF")
{};
virtual ~BlockFFPILimited() {};
float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); }
// accessors
BlockIntegral &getIntegral() { return _integral; }
float getKFF() { return _kFF.get(); }
float getKP() { return _kP.get(); }
float getKI() { return _kI.get(); }
float getOffset() { return _offset.get(); }
BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
protected:
BlockOutputLimiter _outputLimiter;
float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);}
float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) {
float difference = 0.0f;
float integralYPrevious = _integral.getY();
float output = calcUnlimitedOutput(inputValue, inputError);
if(outputLimiter.limit(output, difference) &&
(((difference < 0) && (getKI() * getIntegral().getY() < 0)) ||
((difference > 0) && (getKI() * getIntegral().getY() > 0)))) {
getIntegral().setY(integralYPrevious);
}
return output;
}
private:
BlockIntegral _integral;
BlockParamFloat _kFF;
BlockParamFloat _kP;
BlockParamFloat _kI;
BlockParamFloat _offset;
};
/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/
class BlockFFPILimitedCustom: public BlockFFPILimited
{
public:
// methods
BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
BlockFFPILimited(parent, name, isAngularLimit)
{};
virtual ~BlockFFPILimitedCustom() {};
float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) {
return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter);
}
};
/* A combination of P gain and output limiter */
class BlockPLimited: public SuperBlock
{
public:
// methods
BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
SuperBlock(parent, name),
_kP(this, "P"),
_outputLimiter(this, "", isAngularLimit)
{};
virtual ~BlockPLimited() {};
float update(float input) {
float difference = 0.0f;
float output = getKP() * input;
getOutputLimiter().limit(output, difference);
return output;
}
// accessors
BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
float getKP() { return _kP.get(); }
private:
control::BlockParamFloat _kP;
BlockOutputLimiter _outputLimiter;
};
/* A combination of P, D gains and output limiter */
class BlockPDLimited: public SuperBlock
{
public:
// methods
BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
SuperBlock(parent, name),
_kP(this, "P"),
_kD(this, "D"),
_derivative(this, "D"),
_outputLimiter(this, "", isAngularLimit)
{};
virtual ~BlockPDLimited() {};
float update(float input) {
float difference = 0.0f;
float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f);
getOutputLimiter().limit(output, difference);
return output;
}
// accessors
float getKP() { return _kP.get(); }
float getKD() { return _kD.get(); }
BlockDerivative &getDerivative() { return _derivative; }
BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
private:
control::BlockParamFloat _kP;
control::BlockParamFloat _kD;
BlockDerivative _derivative;
BlockOutputLimiter _outputLimiter;
};
}
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+419
View File
@@ -0,0 +1,419 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@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 mTecs_params.c
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Controller parameters, accessible via MAVLink
*/
/**
* mTECS enabled
*
* Set to 1 to enable mTECS
*
* @min 0
* @max 1
* @group mTECS
*/
PARAM_DEFINE_INT32(MT_ENABLED, 1);
/**
* Total Energy Rate Control Feedforward
* Maps the total energy rate setpoint to the throttle setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f);
/**
* Total Energy Rate Control P
* Maps the total energy rate error to the throttle setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f);
/**
* Total Energy Rate Control I
* Maps the integrated total energy rate to the throttle setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_I, 0.25f);
/**
* Total Energy Rate Control Offset (Cruise throttle sp)
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
/**
* Energy Distribution Rate Control Feedforward
* Maps the energy distribution rate setpoint to the pitch setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f);
/**
* Energy Distribution Rate Control P
* Maps the energy distribution rate error to the pitch setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f);
/**
* Energy Distribution Rate Control I
* Maps the integrated energy distribution rate error to the pitch setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f);
/**
* Total Energy Distribution Offset (Cruise pitch sp)
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f);
/**
* Minimal Throttle Setpoint
*
* @min 0.0
* @max 1.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f);
/**
* Maximal Throttle Setpoint
*
* @min 0.0
* @max 1.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f);
/**
* Minimal Pitch Setpoint in Degrees
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
/**
* Maximal Pitch Setpoint in Degrees
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
* Lowpass (cutoff freq.) for the flight path angle
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
/**
* P gain for the altitude control
* Maps the altitude error to the flight path angle setpoint
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f);
/**
* D gain for the altitude control
* Maps the change of altitude error to the flight path angle setpoint
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
/**
* Lowpass for FPA error derivative calculation (see MT_FPA_D)
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f);
/**
* Minimal flight path angle setpoint
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f);
/**
* Maximal flight path angle setpoint
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
/**
* Lowpass (cutoff freq.) for airspeed
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
/**
* P gain for the airspeed control
* Maps the airspeed error to the acceleration setpoint
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f);
/**
* D gain for the airspeed control
* Maps the change of airspeed error to the acceleration setpoint
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
/**
* Lowpass for ACC error derivative calculation (see MT_ACC_D)
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
/**
* Minimal acceleration (air)
*
* @unit m/s^2
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
/**
* Maximal acceleration (air)
*
* @unit m/s^2
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
/**
* Airspeed derivative calculation lowpass
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
/**
* Minimal throttle during takeoff
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f);
/**
* Maximal throttle during takeoff
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f);
/**
* Minimal pitch during takeoff
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f);
/**
* Maximal pitch during takeoff
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f);
/**
* Minimal throttle in underspeed mode
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f);
/**
* Maximal throttle in underspeed mode
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f);
/**
* Minimal pitch in underspeed mode
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f);
/**
* Maximal pitch in underspeed mode
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f);
/**
* Minimal throttle in landing mode (only last phase of landing)
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f);
/**
* Maximal throttle in landing mode (only last phase of landing)
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f);
/**
* Minimal pitch in landing mode
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f);
/**
* Maximal pitch in landing mode
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f);
/**
* Integrator Limit for Total Energy Rate Control
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f);
/**
* Integrator Limit for Energy Distribution Rate Control
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f);
src/modules/gpio_led/gpio_led.c
+3 -6
View File
@@ -181,16 +181,13 @@ int gpio_led_main(int argc, char *argv[])
} else {
gpio_led_started = true;
warnx("start, using pin: %s", pin_name);
exit(0);
}
exit(0);
} else if (!strcmp(argv[1], "stop")) {
if (gpio_led_started) {
gpio_led_started = false;
warnx("stop");
exit(0);
} else {
errx(1, "not running");
}
@@ -264,7 +261,7 @@ void gpio_led_cycle(FAR void *arg)
pattern = 0x2A; // *_*_*_ fast blink (armed, error)
} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) {
pattern = 0x3f; // ****** solid (armed)
} else {
src/modules/mavlink/mavlink_commands.cpp
+16 -19
View File
@@ -40,34 +40,31 @@
#include "mavlink_commands.h"
MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
{
_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
}
MavlinkCommandsStream::~MavlinkCommandsStream()
{
}
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
if (_cmd_sub->update(t)) {
struct vehicle_command_s cmd;
if (_cmd_sub->update(&_cmd_time, &cmd)) {
/* only send commands for other systems/components */
if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
_cmd->target_system,
_cmd->target_component,
_cmd->command,
_cmd->confirmation,
_cmd->param1,
_cmd->param2,
_cmd->param3,
_cmd->param4,
_cmd->param5,
_cmd->param6,
_cmd->param7);
cmd.target_system,
cmd.target_component,
cmd.command,
cmd.confirmation,
cmd.param1,
cmd.param2,
cmd.param3,
cmd.param4,
cmd.param5,
cmd.param6,
cmd.param7);
}
}
}
src/modules/mavlink/mavlink_commands.h
+1 -1
View File
@@ -55,10 +55,10 @@ private:
MavlinkOrbSubscription *_cmd_sub;
struct vehicle_command_s *_cmd;
mavlink_channel_t _channel;
uint64_t _cmd_time;
public:
MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
~MavlinkCommandsStream();
void update(const hrt_abstime t);
};
src/modules/mavlink/mavlink_ftp.cpp
+415
View File
@@ -0,0 +1,415 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <crc32.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#include "mavlink_ftp.h"
MavlinkFTP *MavlinkFTP::_server;
MavlinkFTP *
MavlinkFTP::getServer()
{
// XXX this really cries out for some locking...
if (_server == nullptr) {
_server = new MavlinkFTP;
}
return _server;
}
MavlinkFTP::MavlinkFTP()
{
// initialise the request freelist
dq_init(&_workFree);
sem_init(&_lock, 0, 1);
// initialize session list
for (size_t i=0; i<kMaxSession; i++) {
_session_fds[i] = -1;
}
// drop work entries onto the free list
for (unsigned i = 0; i < kRequestQueueSize; i++) {
_qFree(&_workBufs[i]);
}
}
void
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
{
// get a free request
auto req = _dqFree();
// if we couldn't get a request slot, just drop it
if (req != nullptr) {
// decode the request
if (req->decode(mavlink, msg)) {
// and queue it for the worker
work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
} else {
_qFree(req);
}
}
}
void
MavlinkFTP::_workerTrampoline(void *arg)
{
auto req = reinterpret_cast<Request *>(arg);
auto server = MavlinkFTP::getServer();
// call the server worker with the work item
server->_worker(req);
}
void
MavlinkFTP::_worker(Request *req)
{
auto hdr = req->header();
ErrorCode errorCode = kErrNone;
uint32_t messageCRC;
// basic sanity checks; must validate length before use
if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
errorCode = kErrNoRequest;
goto out;
}
// check request CRC to make sure this is one of ours
messageCRC = hdr->crc32;
hdr->crc32 = 0;
if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
errorCode = kErrNoRequest;
goto out;
printf("ftp: bad crc\n");
}
printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
switch (hdr->opcode) {
case kCmdNone:
break;
case kCmdTerminate:
errorCode = _workTerminate(req);
break;
case kCmdReset:
errorCode = _workReset();
break;
case kCmdList:
errorCode = _workList(req);
break;
case kCmdOpen:
errorCode = _workOpen(req, false);
break;
case kCmdCreate:
errorCode = _workOpen(req, true);
break;
case kCmdRead:
errorCode = _workRead(req);
break;
case kCmdWrite:
errorCode = _workWrite(req);
break;
case kCmdRemove:
errorCode = _workRemove(req);
break;
default:
errorCode = kErrNoRequest;
break;
}
out:
// handle success vs. error
if (errorCode == kErrNone) {
hdr->opcode = kRspAck;
printf("FTP: ack\n");
} else {
printf("FTP: nak %u\n", errorCode);
hdr->opcode = kRspNak;
hdr->size = 1;
hdr->data[0] = errorCode;
}
// respond to the request
_reply(req);
// free the request buffer back to the freelist
_qFree(req);
}
void
MavlinkFTP::_reply(Request *req)
{
auto hdr = req->header();
// message is assumed to be already constructed in the request buffer, so generate the CRC
hdr->crc32 = 0;
hdr->crc32 = crc32(req->rawData(), req->dataSize());
// then pack and send the reply back to the request source
req->reply();
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workList(Request *req)
{
auto hdr = req->header();
DIR *dp = opendir(req->dataAsCString());
if (dp == nullptr) {
printf("FTP: can't open path '%s'\n", req->dataAsCString());
return kErrNotDir;
}
ErrorCode errorCode = kErrNone;
struct dirent entry, *result = nullptr;
unsigned offset = 0;
// move to the requested offset
seekdir(dp, hdr->offset);
for (;;) {
// read the directory entry
if (readdir_r(dp, &entry, &result)) {
errorCode = kErrIO;
break;
}
// no more entries?
if (result == nullptr) {
if (hdr->offset != 0 && offset == 0) {
// User is requesting subsequent dir entries but there were none. This means the user asked
// to seek past EOF.
errorCode = kErrEOF;
}
// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
break;
}
// name too big to fit?
if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
break;
}
// store the type marker
switch (entry.d_type) {
case DTYPE_FILE:
hdr->data[offset++] = kDirentFile;
break;
case DTYPE_DIRECTORY:
hdr->data[offset++] = kDirentDir;
break;
default:
hdr->data[offset++] = kDirentUnknown;
break;
}
// copy the name, which we know will fit
strcpy((char *)&hdr->data[offset], entry.d_name);
offset += strlen(entry.d_name) + 1;
printf("FTP: list %s\n", entry.d_name);
}
closedir(dp);
hdr->size = offset;
return errorCode;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(Request *req, bool create)
{
auto hdr = req->header();
int session_index = _findUnusedSession();
if (session_index < 0) {
return kErrNoSession;
}
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
int fd = ::open(req->dataAsCString(), oflag);
if (fd < 0) {
return create ? kErrPerm : kErrNotFile;
}
_session_fds[session_index] = fd;
hdr->session = session_index;
hdr->size = 0;
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workRead(Request *req)
{
auto hdr = req->header();
int session_index = hdr->session;
if (!_validSession(session_index)) {
return kErrNoSession;
}
// Seek to the specified position
printf("Seek %d\n", hdr->offset);
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
return kErrEOF;
}
int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
return kErrIO;
}
printf("Read success %d\n", bytes_read);
hdr->size = bytes_read;
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(Request *req)
{
#if 0
// NYI: Coming soon
auto hdr = req->header();
// look up session
auto session = getSession(hdr->session);
if (session == nullptr) {
return kErrNoSession;
}
// append to file
int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
if (result < 0) {
// XXX might also be no space, I/O, etc.
return kErrNotAppend;
}
hdr->size = result;
return kErrNone;
#else
return kErrPerm;
#endif
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemove(Request *req)
{
auto hdr = req->header();
// for now, send error reply
return kErrPerm;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(Request *req)
{
auto hdr = req->header();
if (!_validSession(hdr->session)) {
return kErrNoSession;
}
::close(_session_fds[hdr->session]);
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workReset(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] != -1) {
::close(_session_fds[i]);
_session_fds[i] = -1;
}
}
return kErrNone;
}
bool
MavlinkFTP::_validSession(unsigned index)
{
if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
return false;
}
return true;
}
int
MavlinkFTP::_findUnusedSession(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] == -1) {
return i;
}
}
return -1;
}
char *
MavlinkFTP::Request::dataAsCString()
{
// guarantee nul termination
if (header()->size < kMaxDataLength) {
requestData()[header()->size] = '\0';
} else {
requestData()[kMaxDataLength - 1] = '\0';
}
// and return data
return (char *)&(header()->data[0]);
}
src/modules/mavlink/mavlink_ftp.h
+226
View File
@@ -0,0 +1,226 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
/**
* @file mavlink_ftp.h
*
* MAVLink remote file server.
*
* Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
* a session ID and sequence number.
*
* A limited number of requests (currently 2) may be outstanding at a time.
* Additional messages will be discarded.
*
* Messages consist of a fixed header, followed by a data area.
*
*/
#include <dirent.h>
#include <queue.h>
#include <nuttx/wqueue.h>
#include <systemlib/err.h>
#include "mavlink_messages.h"
class MavlinkFTP
{
public:
MavlinkFTP();
static MavlinkFTP *getServer();
// static interface
void handle_message(Mavlink* mavlink,
mavlink_message_t *msg);
private:
static const unsigned kRequestQueueSize = 2;
static MavlinkFTP *_server;
struct RequestHeader
{
uint8_t magic;
uint8_t session;
uint8_t opcode;
uint8_t size;
uint32_t crc32;
uint32_t offset;
uint8_t data[];
};
enum Opcode : uint8_t
{
kCmdNone, // ignored, always acked
kCmdTerminate, // releases sessionID, closes file
kCmdReset, // terminates all sessions
kCmdList, // list files in <path> from <offset>
kCmdOpen, // opens <path> for reading, returns <session>
kCmdRead, // reads <size> bytes from <offset> in <session>
kCmdCreate, // creates <path> for writing, returns <session>
kCmdWrite, // appends <size> bytes at <offset> in <session>
kCmdRemove, // remove file (only if created by server?)
kRspAck,
kRspNak
};
enum ErrorCode : uint8_t
{
kErrNone,
kErrNoRequest,
kErrNoSession,
kErrSequence,
kErrNotDir,
kErrNotFile,
kErrEOF,
kErrNotAppend,
kErrTooBig,
kErrIO,
kErrPerm
};
int _findUnusedSession(void);
bool _validSession(unsigned index);
static const unsigned kMaxSession = 2;
int _session_fds[kMaxSession];
class Request
{
public:
union {
dq_entry_t entry;
work_s work;
};
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
_mavlink = mavlink;
mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
return true;
}
return false;
}
void reply() {
// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
// flat memory architecture, as we're operating between threads here.
mavlink_message_t msg;
msg.checksum = 0;
unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
_mavlink->get_channel(), &msg, sequence(), rawData());
_mavlink->lockMessageBufferMutex();
bool fError = _mavlink->message_buffer_write(&msg, len);
_mavlink->unlockMessageBufferMutex();
if (!fError) {
warnx("FTP TX ERR");
} else {
warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
_mavlink->get_system_id(),
_mavlink->get_component_id(),
_mavlink->get_channel(),
len,
msg.checksum);
}
}
uint8_t *rawData() { return &_message.data[0]; }
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
uint8_t *requestData() { return &(header()->data[0]); }
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
uint16_t sequence() const { return _message.seqnr; }
mavlink_channel_t channel() { return _mavlink->get_channel(); }
char *dataAsCString();
private:
Mavlink *_mavlink;
mavlink_encapsulated_data_t _message;
};
static const uint8_t kProtocolMagic = 'f';
static const char kDirentFile = 'F';
static const char kDirentDir = 'D';
static const char kDirentUnknown = 'U';
static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
/// Request worker; runs on the low-priority work queue to service
/// remote requests.
///
static void _workerTrampoline(void *arg);
void _worker(Request *req);
/// Reply to a request (XXX should be a Request method)
///
void _reply(Request *req);
ErrorCode _workList(Request *req);
ErrorCode _workOpen(Request *req, bool create);
ErrorCode _workRead(Request *req);
ErrorCode _workWrite(Request *req);
ErrorCode _workRemove(Request *req);
ErrorCode _workTerminate(Request *req);
ErrorCode _workReset();
// work freelist
Request _workBufs[kRequestQueueSize];
dq_queue_t _workFree;
sem_t _lock;
void _qLock() { do {} while (sem_wait(&_lock) != 0); }
void _qUnlock() { sem_post(&_lock); }
void _qFree(Request *req) {
_qLock();
dq_addlast(&req->entry, &_workFree);
_qUnlock();
}
Request *_dqFree() {
_qLock();
auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
_qUnlock();
return req;
}
};
src/modules/mavlink/mavlink_main.cpp
+168 -114
View File
@@ -83,6 +83,10 @@
#include "mavlink_rate_limiter.h"
#include "mavlink_commands.h"
#ifndef MAVLINK_CRC_EXTRA
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
#endif
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0};
void
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
Mavlink *instance;
switch (channel) {
@@ -192,13 +197,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
if (buf_free < desired) {
/* we don't want to send anything just in half, so return */
instance->count_txerr();
return;
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
warnx("TX FAIL");
instance->count_txerr();
} else {
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
@@ -230,14 +236,22 @@ Mavlink::Mavlink() :
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
_uart_fd(-1),
_mavlink_param_queue_index(0),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_message_buffer({}),
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(0),
_param_use_hil_gps(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
_wpm = &_wpm_s;
mission.count = 0;
@@ -290,6 +304,7 @@ Mavlink::Mavlink() :
Mavlink::~Mavlink()
{
perf_free(_loop_perf);
perf_free(_txerr_perf);
if (_task_running) {
/* task wakes up every 10ms or so at the longest */
@@ -314,6 +329,12 @@ Mavlink::~Mavlink()
LL_DELETE(_mavlink_instances, this);
}
void
Mavlink::count_txerr()
{
perf_count(_txerr_perf);
}
void
Mavlink::set_mode(enum MAVLINK_MODE mode)
{
@@ -494,48 +515,53 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
void Mavlink::mavlink_update_system(void)
{
static bool initialized = false;
static param_t param_system_id;
static param_t param_component_id;
static param_t param_system_type;
static param_t param_use_hil_gps;
if (!initialized) {
param_system_id = param_find("MAV_SYS_ID");
param_component_id = param_find("MAV_COMP_ID");
param_system_type = param_find("MAV_TYPE");
param_use_hil_gps = param_find("MAV_USEHILGPS");
initialized = true;
if (!_param_initialized) {
_param_system_id = param_find("MAV_SYS_ID");
_param_component_id = param_find("MAV_COMP_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_initialized = true;
}
/* update system and component id */
int32_t system_id;
param_get(param_system_id, &system_id);
param_get(_param_system_id, &system_id);
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
int32_t component_id;
param_get(param_component_id, &component_id);
param_get(_param_component_id, &component_id);
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
}
int32_t system_type;
param_get(param_system_type, &system_type);
param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
int32_t use_hil_gps;
param_get(param_use_hil_gps, &use_hil_gps);
param_get(_param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
}
int Mavlink::get_system_id()
{
return mavlink_system.sysid;
}
int Mavlink::get_component_id()
{
return mavlink_system.compid;
}
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
/* process baud rate */
@@ -819,7 +845,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[mavlink pm] unknown: %s", name);
sprintf(buf, "[pm] unknown: %s", name);
mavlink_missionlib_send_gcs_string(buf);
} else {
@@ -898,7 +924,11 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param1;
break;
case MAV_CMD_DO_JUMP:
mission_item->do_jump_mission_index = mavlink_mission_item->param1;
mission_item->do_jump_current_count = 0;
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
@@ -914,6 +944,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
/* reset DO_JUMP count */
mission_item->do_jump_current_count = 0;
return OK;
}
@@ -931,6 +964,11 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
mavlink_mission_item->param1 = mission_item->pitch_min;
break;
case NAV_CMD_DO_JUMP:
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
mavlink_mission_item->param1 = mission_item->time_inside;
@@ -1008,8 +1046,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
} else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
if (_verbose) { warnx("ERROR: index out of bounds"); }
}
}
@@ -1082,8 +1118,6 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
} else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); }
}
}
@@ -1114,8 +1148,6 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
mavlink_missionlib_send_gcs_string("Operation timeout");
if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); }
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
_wpm->current_partner_sysid = 0;
_wpm->current_partner_compid = 0;
@@ -1150,8 +1182,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); }
}
break;
@@ -1175,15 +1205,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); }
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
}
}
@@ -1214,8 +1239,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
}
}
@@ -1233,8 +1256,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); }
break;
}
@@ -1245,15 +1266,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpr.seq == 0) {
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
if (_verbose) { warnx("REJ. WP CMD: First id != 0"); }
break;
}
@@ -1261,17 +1280,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq == _wpm->current_wp_id) {
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else if (wpr.seq == _wpm->current_wp_id + 1) {
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); }
break;
}
@@ -1279,8 +1296,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); }
break;
}
@@ -1294,7 +1309,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); }
mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds");
}
@@ -1328,15 +1343,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
}
if (wpc.count == 0) {
mavlink_missionlib_send_gcs_string("COUNT 0");
if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); }
mavlink_missionlib_send_gcs_string("WP COUNT 0");
break;
}
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
_wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
_wpm->current_wp_id = 0;
_wpm->current_partner_sysid = msg->sysid;
@@ -1350,18 +1361,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_wpm->current_wp_id == 0) {
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); }
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP");
}
} else {
mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
}
}
}
@@ -1384,7 +1389,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wp.seq != 0) {
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
break;
}
@@ -1392,12 +1396,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wp.seq >= _wpm->current_count) {
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq);
break;
}
if (wp.seq != _wpm->current_wp_id) {
warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id);
mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch");
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
break;
}
@@ -1512,11 +1515,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(_channel, buf, len);
}
@@ -1590,31 +1592,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
/* delete stream */
LL_DELETE(_streams, stream);
delete stream;
warnx("deleted stream %s", stream->get_name());
}
return OK;
}
}
if (interval > 0) {
/* search for stream with specified name in supported streams list */
for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
/* create new instance */
stream = streams_list[i]->new_instance();
stream->set_channel(get_channel());
stream->set_interval(interval);
stream->subscribe(this);
LL_APPEND(_streams, stream);
return OK;
}
}
} else {
/* stream not found, nothing to disable */
if (interval == 0) {
/* stream was not active and is requested to be disabled, do nothing */
return OK;
}
/* search for stream with specified name in supported streams list */
for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
/* create new instance */
stream = streams_list[i]->new_instance();
stream->set_channel(get_channel());
stream->set_interval(interval);
stream->subscribe(this);
LL_APPEND(_streams, stream);
return OK;
}
}
/* if we reach here, the stream list does not contain the stream */
warnx("stream %s not found", stream_name);
return ERROR;
}
@@ -1649,11 +1656,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
int
Mavlink::message_buffer_init(int size)
{
_message_buffer.size = size;
_message_buffer.write_ptr = 0;
_message_buffer.read_ptr = 0;
_message_buffer.data = (char*)malloc(_message_buffer.size);
return (_message_buffer.data == 0) ? ERROR : OK;
int ret;
if (_message_buffer.data == 0) {
ret = ERROR;
_message_buffer.size = 0;
} else {
ret = OK;
}
return ret;
}
void
@@ -1781,7 +1798,7 @@ Mavlink::task_main(int argc, char *argv[])
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
@@ -1837,6 +1854,10 @@ Mavlink::task_main(int argc, char *argv[])
_wait_to_transmit = true;
break;
case 'x':
_ftp_on = true;
break;
default:
err_flag = true;
break;
@@ -1902,9 +1923,12 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_logbuffer_init(&_logbuffer, 5);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_passing_on) {
/* initialize message buffer if multiplexing is on */
if (OK != message_buffer_init(500)) {
if (_passing_on || _ftp_on) {
/* initialize message buffer if multiplexing is on or its needed for FTP.
* make space for two messages plus off-by-one space as we use the empty element
* marker ring buffer approach.
*/
if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
errx(1, "can't allocate message buffer, exiting");
}
@@ -1934,9 +1958,12 @@ Mavlink::task_main(int argc, char *argv[])
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
uint64_t param_time = 0;
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
uint64_t status_time = 0;
struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
struct vehicle_status_s status;
status_sub->update(&status_time, &status);
MavlinkCommandsStream commands_stream(this, _channel);
@@ -1993,14 +2020,14 @@ Mavlink::task_main(int argc, char *argv[])
hrt_abstime t = hrt_absolute_time();
if (param_sub->update(t)) {
if (param_sub->update(&param_time, nullptr)) {
/* parameters updated */
mavlink_update_system();
}
if (status_sub->update(t)) {
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
set_hil_enabled(status->hil_state == HIL_STATE_ON);
set_hil_enabled(status.hil_state == HIL_STATE_ON);
}
/* update commands stream */
@@ -2064,32 +2091,50 @@ Mavlink::task_main(int argc, char *argv[])
}
}
/* pass messages from other UARTs */
if (_passing_on) {
/* pass messages from other UARTs or FTP worker */
if (_passing_on || _ftp_on) {
bool is_part;
void *read_ptr;
uint8_t *read_ptr;
uint8_t *write_ptr;
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr(&read_ptr, &is_part);
int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
/* write first part of buffer */
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
message_buffer_mark_read(available);
// Reconstruct message from buffer
mavlink_message_t msg;
write_ptr = (uint8_t*)&msg;
// Pull a single message from the buffer
size_t read_count = available;
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
memcpy(write_ptr, read_ptr, read_count);
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_mark_read(read_count);
/* write second part of buffer if there is some */
if (is_part) {
/* guard get ptr by mutex */
pthread_mutex_lock(&_message_buffer_mutex);
available = message_buffer_get_ptr(&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
if (is_part && read_count < sizeof(mavlink_message_t)) {
write_ptr += read_count;
available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
read_count = sizeof(mavlink_message_t) - read_count;
memcpy(write_ptr, read_ptr, read_count);
message_buffer_mark_read(available);
}
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
}
}
@@ -2139,7 +2184,7 @@ Mavlink::task_main(int argc, char *argv[])
/* close mavlink logging device */
close(_mavlink_fd);
if (_passing_on) {
if (_passing_on || _ftp_on) {
message_buffer_destroy();
pthread_mutex_destroy(&_message_buffer_mutex);
}
@@ -2157,11 +2202,20 @@ int Mavlink::start_helper(int argc, char *argv[])
/* create the instance in task context */
Mavlink *instance = new Mavlink();
/* this will actually only return once MAVLink exits */
int res = instance->task_main(argc, argv);
int res;
/* delete instance on main thread end */
delete instance;
if (!instance) {
/* out of memory */
res = -ENOMEM;
warnx("OUT OF MEM");
} else {
/* this will actually only return once MAVLink exits */
res = instance->task_main(argc, argv);
/* delete instance on main thread end */
delete instance;
}
return res;
}
@@ -2212,13 +2266,13 @@ Mavlink::start(int argc, char *argv[])
}
void
Mavlink::status()
Mavlink::display_status()
{
warnx("running");
}
int
Mavlink::stream(int argc, char *argv[])
Mavlink::stream_command(int argc, char *argv[])
{
const char *device_name = DEFAULT_DEVICE_NAME;
float rate = -1.0f;
@@ -2281,7 +2335,7 @@ Mavlink::stream(int argc, char *argv[])
static void usage()
{
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
}
int mavlink_main(int argc, char *argv[])
@@ -2306,7 +2360,7 @@ int mavlink_main(int argc, char *argv[])
// mavlink::g_mavlink->status();
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream(argc, argv);
return Mavlink::stream_command(argc, argv);
} else {
usage();
src/modules/mavlink/mavlink_main.h
+113 -79
View File
@@ -123,27 +123,41 @@ public:
/**
* Display the mavlink status.
*/
void status();
void display_status();
static int stream(int argc, char *argv[]);
static int stream_command(int argc, char *argv[]);
static int instance_count();
static int instance_count();
static Mavlink *new_instance();
static Mavlink *new_instance();
static Mavlink *get_instance(unsigned instance);
static Mavlink *get_instance(unsigned instance);
static Mavlink *get_instance_for_device(const char *device_name);
static Mavlink *get_instance_for_device(const char *device_name);
static int destroy_all_instances();
static int destroy_all_instances();
static bool instance_exists(const char *device_name, Mavlink *self);
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(mavlink_message_t *msg, Mavlink *self);
static void forward_message(mavlink_message_t *msg, Mavlink *self);
static int get_uart_fd(unsigned index);
static int get_uart_fd(unsigned index);
int get_uart_fd();
int get_uart_fd();
/**
* Get the MAVLink system id.
*
* @return The system ID of this vehicle
*/
int get_system_id();
/**
* Get the MAVLink component id.
*
* @return The component ID of this vehicle
*/
int get_component_id();
const char *_device_name;
@@ -153,30 +167,30 @@ public:
MAVLINK_MODE_CAMERA
};
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
bool get_hil_enabled() { return _hil_enabled; }
bool get_hil_enabled() { return _hil_enabled; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
bool get_forwarding_on() { return _forwarding_on; }
/**
* Handle waypoint related messages.
*/
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
static int start_helper(int argc, char *argv[]);
static int start_helper(int argc, char *argv[]);
/**
* Handle parameter related messages.
*/
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
/**
* Enable / disable Hardware in the Loop simulation mode.
@@ -186,90 +200,105 @@ public:
* requested change could not be made or was
* redundant.
*/
int set_hil_enabled(bool hil_enabled);
int set_hil_enabled(bool hil_enabled);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
int get_instance_id();
int get_instance_id();
/**
* Enable / disable hardware flow control.
*
* @param enabled True if hardware flow control should be enabled
*/
int enable_flow_control(bool enabled);
int enable_flow_control(bool enabled);
mavlink_channel_t get_channel();
mavlink_channel_t get_channel();
bool _task_should_exit; /**< if true, mavlink task should exit */
void configure_stream_threadsafe(const char *stream_name, float rate);
int get_mavlink_fd() { return _mavlink_fd; }
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
MavlinkStream * get_streams() const { return _streams; }
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
bool message_buffer_write(void *ptr, int size);
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
/**
* Count a transmision error
*/
void count_txerr();
protected:
Mavlink *next;
Mavlink *next;
private:
int _instance_id;
int _instance_id;
int _mavlink_fd;
bool _task_running;
int _mavlink_fd;
bool _task_running;
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
orb_advert_t _mission_pub;
struct mission_s mission;
MAVLINK_MODE _mode;
orb_advert_t _mission_pub;
struct mission_s mission;
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
mavlink_channel_t _channel;
uint8_t _mavlink_wpm_comp_id;
mavlink_channel_t _channel;
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
unsigned int _total_counter;
pthread_t _receive_thread;
pthread_t _receive_thread;
/* Allocate storage space for waypoints */
mavlink_wpm_storage _wpm_s;
mavlink_wpm_storage *_wpm;
mavlink_wpm_storage _wpm_s;
mavlink_wpm_storage *_wpm;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
int _uart_fd;
int _baudrate;
int _datarate;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
bool _ftp_on;
int _uart_fd;
int _baudrate;
int _datarate;
/**
* If the queue index is not at 0, the queue sending
* logic will send parameters from the current index
* to len - 1, the end of the param list.
*/
unsigned int _mavlink_param_queue_index;
unsigned int _mavlink_param_queue_index;
bool mavlink_link_termination_allowed;
bool mavlink_link_termination_allowed;
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
bool _flow_control_enabled;
struct mavlink_message_buffer {
int write_ptr;
@@ -277,11 +306,19 @@ private:
int size;
char *data;
};
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
mavlink_message_buffer _message_buffer;
perf_counter_t _loop_perf; /**< loop performance counter */
pthread_mutex_t _message_buffer_mutex;
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
/**
* Send one parameter.
@@ -289,7 +326,7 @@ private:
* @param param The parameter id to send.
* @return zero on success, nonzero on failure.
*/
int mavlink_pm_send_param(param_t param);
int mavlink_pm_send_param(param_t param);
/**
* Send one parameter identified by index.
@@ -297,7 +334,7 @@ private:
* @param index The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_index(uint16_t index);
int mavlink_pm_send_param_for_index(uint16_t index);
/**
* Send one parameter identified by name.
@@ -305,14 +342,14 @@ private:
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_name(const char *name);
int mavlink_pm_send_param_for_name(const char *name);
/**
* Send a queue of parameters, one parameter per function call.
*
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send(void);
int mavlink_pm_queued_send(void);
/**
* Start sending the parameter queue.
@@ -322,12 +359,12 @@ private:
* mavlink_pm_queued_send().
* @see mavlink_pm_queued_send()
*/
void mavlink_pm_start_queued_send();
void mavlink_pm_start_queued_send();
void mavlink_update_system();
void mavlink_update_system();
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
@@ -344,7 +381,6 @@ private:
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
int configure_stream(const char *stream_name, const float rate);
void configure_stream_threadsafe(const char *stream_name, const float rate);
int message_buffer_init(int size);
@@ -354,8 +390,6 @@ private:
int message_buffer_is_empty();
bool message_buffer_write(void *ptr, int size);
int message_buffer_get_ptr(void **ptr, bool *is_part);
void message_buffer_mark_read(int n);
src/modules/mavlink/mavlink_messages.cpp
+780 -382
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_messages.h
+14 -1
View File
@@ -43,6 +43,19 @@
#include "mavlink_stream.h"
extern MavlinkStream *streams_list[];
class StreamListItem {
public:
MavlinkStream* (*new_instance)();
const char* (*get_name)();
StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
new_instance(inst),
get_name(name) {};
~StreamListItem() {};
};
extern StreamListItem *streams_list[];
#endif /* MAVLINK_MESSAGES_H_ */
src/modules/mavlink/mavlink_orb_subscription.cpp
+33 -28
View File
@@ -47,53 +47,58 @@
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
_fd(orb_subscribe(_topic)),
_published(false),
next(nullptr),
_topic(topic),
_last_check(0),
next(nullptr)
_fd(orb_subscribe(_topic)),
_published(false)
{
_data = malloc(topic->o_size);
memset(_data, 0, topic->o_size);
}
MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
close(_fd);
free(_data);
}
orb_id_t
MavlinkOrbSubscription::get_topic()
MavlinkOrbSubscription::get_topic() const
{
return _topic;
}
void *
MavlinkOrbSubscription::get_data()
bool
MavlinkOrbSubscription::update(uint64_t *time, void* data)
{
return _data;
// TODO this is NOT atomic operation, we can get data newer than time
// if topic was published between orb_stat and orb_copy calls.
uint64_t time_topic;
if (orb_stat(_fd, &time_topic)) {
/* error getting last topic publication time */
time_topic = 0;
}
if (orb_copy(_topic, _fd, data)) {
/* error copying topic data */
memset(data, 0, _topic->o_size);
return false;
} else {
/* data copied successfully */
_published = true;
if (time_topic != *time) {
*time = time_topic;
return true;
} else {
return false;
}
}
}
bool
MavlinkOrbSubscription::update(const hrt_abstime t)
MavlinkOrbSubscription::update(void* data)
{
if (_last_check == t) {
/* already checked right now, return result of the check */
return _updated;
} else {
_last_check = t;
orb_check(_fd, &_updated);
if (_updated) {
orb_copy(_topic, _fd, _data);
_published = true;
return true;
}
}
return false;
return !orb_copy(_topic, _fd, data);
}
bool
src/modules/mavlink/mavlink_orb_subscription.h
+20 -10
View File
@@ -48,12 +48,26 @@
class MavlinkOrbSubscription
{
public:
MavlinkOrbSubscription *next; /*< pointer to next subscription in list */
MavlinkOrbSubscription *next; ///< pointer to next subscription in list
MavlinkOrbSubscription(const orb_id_t topic);
~MavlinkOrbSubscription();
bool update(const hrt_abstime t);
/**
* Check if subscription updated and get data.
*
* @return true only if topic was updated and data copied to buffer successfully.
* If topic was not updated since last check it will return false but still copy the data.
* If no data available data buffer will be filled with zeroes.
*/
bool update(uint64_t *time, void* data);
/**
* Copy topic data to given buffer.
*
* @return true only if topic data copied successfully.
*/
bool update(void* data);
/**
* Check if the topic has been published.
@@ -62,16 +76,12 @@ public:
* @return true if the topic has been published at least once.
*/
bool is_published();
void *get_data();
orb_id_t get_topic();
orb_id_t get_topic() const;
private:
const orb_id_t _topic; /*< topic metadata */
int _fd; /*< subscription handle */
bool _published; /*< topic was ever published */
void *_data; /*< pointer to data buffer */
hrt_abstime _last_check; /*< time of last check */
bool _updated; /*< updated on last check */
const orb_id_t _topic; ///< topic metadata
int _fd; ///< subscription handle
bool _published; ///< topic was ever published
};
src/modules/mavlink/mavlink_receiver.cpp
+72 -4
View File
@@ -106,12 +106,17 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_telemetry_heartbeat_time(0),
_radio_status_available(false),
_hil_frames(0),
_old_timestamp(0),
_hil_local_proj_inited(0),
_hil_local_alt0(0.0)
{
memset(&hil_local_pos, 0, sizeof(hil_local_pos));
// make sure the FTP server is started
(void)MavlinkFTP::getServer();
}
MavlinkReceiver::~MavlinkReceiver()
@@ -150,6 +155,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_manual_control(msg);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
handle_message_heartbeat(msg);
break;
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
handle_message_request_data_stream(msg);
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
break;
default:
break;
}
@@ -411,6 +428,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
@@ -426,6 +444,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
}
void
@@ -451,6 +472,54 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
{
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
/* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
_telemetry_heartbeat_time = hrt_absolute_time();
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
}
}
}
void
MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
{
mavlink_request_data_stream_t req;
mavlink_msg_request_data_stream_decode(msg, &req);
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
MavlinkStream *stream;
LL_FOREACH(_mavlink->get_streams(), stream) {
if (req.req_stream_id == stream->get_id()) {
_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
}
}
}
}
void
MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
{
@@ -667,12 +736,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.lat = gps.lat;
hil_gps.lon = gps.lon;
hil_gps.alt = gps.alt;
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
hil_gps.timestamp_variance = timestamp;
hil_gps.s_variance_m_s = 5.0f;
hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
hil_gps.timestamp_velocity = timestamp;
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
@@ -952,7 +1021,6 @@ MavlinkReceiver::receive_start(Mavlink *parent)
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2900);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
src/modules/mavlink/mavlink_receiver.h
+6
View File
@@ -68,6 +68,8 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include "mavlink_ftp.h"
class Mavlink;
class MavlinkReceiver
@@ -112,6 +114,8 @@ private:
void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
void handle_message_radio_status(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
void handle_message_heartbeat(mavlink_message_t *msg);
void handle_message_request_data_stream(mavlink_message_t *msg);
void handle_message_hil_sensor(mavlink_message_t *msg);
void handle_message_hil_gps(mavlink_message_t *msg);
void handle_message_hil_state_quaternion(mavlink_message_t *msg);
@@ -138,6 +142,8 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
hrt_abstime _telemetry_heartbeat_time;
bool _radio_status_available;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;
src/modules/mavlink/mavlink_stream.cpp
+5 -1
View File
@@ -43,7 +43,11 @@
#include "mavlink_stream.h"
#include "mavlink_main.h"
MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
MavlinkStream::MavlinkStream() :
next(nullptr),
_channel(MAVLINK_COMM_0),
_interval(1000000),
_last_sent(0)
{
}
src/modules/mavlink/mavlink_stream.h
+13 -10
View File
@@ -50,14 +50,6 @@ class MavlinkStream;
class MavlinkStream
{
private:
hrt_abstime _last_sent;
protected:
mavlink_channel_t _channel;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
public:
MavlinkStream *next;
@@ -71,9 +63,20 @@ public:
* @return 0 if updated / sent, -1 if unchanged
*/
int update(const hrt_abstime t);
virtual MavlinkStream *new_instance() = 0;
static MavlinkStream *new_instance();
static const char *get_name_static();
virtual void subscribe(Mavlink *mavlink) = 0;
virtual const char *get_name() = 0;
virtual const char *get_name() const = 0;
virtual uint8_t get_id() = 0;
protected:
mavlink_channel_t _channel;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
private:
hrt_abstime _last_sent;
};
src/modules/mavlink/module.mk
+2 -1
View File
@@ -43,7 +43,8 @@ SRCS += mavlink_main.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \
mavlink_rate_limiter.cpp \
mavlink_commands.cpp
mavlink_commands.cpp \
mavlink_ftp.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
src/modules/mc_att_control/mc_att_control_main.cpp
+13 -4
View File
@@ -72,6 +72,7 @@
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
@@ -123,6 +124,8 @@ private:
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
@@ -267,6 +270,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_v_rates_sp_pub(-1),
_actuators_0_pub(-1),
_actuators_0_circuit_breaker_enabled(false),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
@@ -402,6 +407,8 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
}
@@ -840,11 +847,13 @@ MulticopterAttitudeControl::task_main()
_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
}
}
}
src/modules/mc_pos_control/mc_pos_control_main.cpp
+2 -3
View File
@@ -466,7 +466,7 @@ MulticopterPositionControl::update_ref()
{
if (_local_pos.ref_timestamp != _ref_timestamp) {
double lat_sp, lon_sp;
float alt_sp;
float alt_sp = 0.0f;
if (_ref_timestamp != 0) {
/* calculate current position setpoint in global frame */
@@ -545,7 +545,6 @@ MulticopterPositionControl::task_main()
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
math::Vector<3> sp_move_rate;
sp_move_rate.zero();
@@ -862,7 +861,7 @@ MulticopterPositionControl::task_main()
if (_control_mode.flag_control_velocity_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
src/modules/navigator/geofence.cpp
+5 -4
View File
@@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
{
double lat = vehicle->lat / 1e7d;
double lon = vehicle->lon / 1e7d;
float alt = vehicle->alt;
//float alt = vehicle->alt;
return inside(lat, lon, vehicle->alt);
}
@@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
}
// skip vertex 0 (return point)
if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
(temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
(lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
(double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
c = !c;
}
@@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename)
int Geofence::clearDm()
{
dm_clear(DM_KEY_FENCE_POINTS);
return OK;
}
src/modules/navigator/loiter.cpp
+78
View File
@@ -0,0 +1,78 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file loiter.cpp
*
* Helper class to loiter
*
* @author Julian Oes <julian@oes.ch>
*/
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include "loiter.h"
Loiter::Loiter(Navigator *navigator, const char *name) :
MissionBlock(navigator, name)
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
Loiter::~Loiter()
{
}
bool
Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* set loiter item, don't reuse an existing position setpoint */
return set_loiter_item(pos_sp_triplet);
}
void
Loiter::on_inactive()
{
}
src/modules/navigator/loiter.h
+74
View File
@@ -0,0 +1,74 @@
/***************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file loiter.h
*
* Helper class to loiter
*
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_LOITER_H
#define NAVIGATOR_LOITER_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include "navigator_mode.h"
#include "mission_block.h"
class Loiter : public MissionBlock
{
public:
/**
* Constructor
*/
Loiter(Navigator *navigator, const char *name);
/**
* Destructor
*/
~Loiter();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called while the mode is active
*/
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
};
#endif
src/modules/navigator/mission.cpp
+461
View File
@@ -0,0 +1,461 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mission.cpp
*
* Helper class to access missions
*
* @author Julian Oes <julian@oes.ch>
*/
#include <sys/types.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include "navigator.h"
#include "mission.h"
Mission::Mission(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"),
_onboard_mission({0}),
_offboard_mission({0}),
_current_onboard_mission_index(-1),
_current_offboard_mission_index(-1),
_mission_result_pub(-1),
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE)
{
/* load initial params */
updateParams();
/* set initial mission items */
on_inactive();
}
Mission::~Mission()
{
}
void
Mission::on_inactive()
{
_first_run = true;
/* check anyway if missions have changed so that feedback to groundstation is given */
bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
}
bool
Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
bool updated = false;
/* check if anything has changed */
bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
/* reset mission items if needed */
if (onboard_updated || offboard_updated || _first_run) {
set_mission_items(pos_sp_triplet);
updated = true;
_first_run = false;
}
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
advance_mission();
set_mission_items(pos_sp_triplet);
updated = true;
}
return updated;
}
void
Mission::update_onboard_mission()
{
if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
/* accept the current index set by the onboard mission if it is within bounds */
if (_onboard_mission.current_index >=0
&& _onboard_mission.current_index < (int)_onboard_mission.count) {
_current_onboard_mission_index = _onboard_mission.current_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
_current_onboard_mission_index = 0;
/* if not initialized, set it to 0 */
} else if (_current_onboard_mission_index < 0) {
_current_onboard_mission_index = 0;
}
/* otherwise, just leave it */
}
} else {
_onboard_mission.count = 0;
_onboard_mission.current_index = 0;
_current_onboard_mission_index = 0;
}
}
void
Mission::update_offboard_mission()
{
if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
/* determine current index */
if (_offboard_mission.current_index >= 0
&& _offboard_mission.current_index < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0;
/* if not initialized, set it to 0 */
} else if (_current_offboard_mission_index < 0) {
_current_offboard_mission_index = 0;
}
/* otherwise, just leave it */
}
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
(size_t)_offboard_mission.count,
_navigator->get_geofence(),
_navigator->get_home_position()->alt);
} else {
_offboard_mission.count = 0;
_offboard_mission.current_index = 0;
_current_offboard_mission_index = 0;
}
report_current_offboard_mission_item();
}
void
Mission::advance_mission()
{
switch (_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
}
}
void
Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
{
set_previous_pos_setpoint(pos_sp_triplet);
/* try setting onboard mission item */
if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: onboard mission running");
}
_mission_type = MISSION_TYPE_ONBOARD;
_navigator->set_can_loiter_at_sp(false);
/* try setting offboard mission item */
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: offboard mission running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
_navigator->set_can_loiter_at_sp(false);
} else {
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: mission finished");
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: no mission available");
}
_mission_type = MISSION_TYPE_NONE;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
set_loiter_item(pos_sp_triplet);
reset_mission_item_reached();
report_mission_finished();
}
}
bool
Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{
/* make sure param is up to date */
updateParams();
if (_param_onboard_enabled.get() > 0 &&
_current_onboard_mission_index >= 0&&
_current_onboard_mission_index < (int)_onboard_mission.count) {
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
&new_mission_item)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
current_pos_sp->valid = true;
reset_mission_item_reached();
/* TODO: report this somehow */
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
return true;
}
}
return false;
}
bool
Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{
if (_current_offboard_mission_index >= 0 &&
_current_offboard_mission_index < (int)_offboard_mission.count) {
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
struct mission_item_s new_mission_item;
if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
current_pos_sp->valid = true;
reset_mission_item_reached();
report_current_offboard_mission_item();
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
return true;
}
}
return false;
}
void
Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
{
int next_temp_mission_index = _onboard_mission.current_index + 1;
/* try if there is a next onboard mission */
if (_onboard_mission.current_index >= 0 &&
next_temp_mission_index < (int)_onboard_mission.count) {
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
}
}
/* give up */
next_pos_sp->valid = false;
return;
}
void
Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
{
/* try if there is a next offboard mission */
int next_temp_mission_index = _offboard_mission.current_index + 1;
warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
if (_offboard_mission.current_index >= 0 &&
next_temp_mission_index < (int)_offboard_mission.count) {
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
struct mission_item_s new_mission_item;
if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
}
}
/* give up */
next_pos_sp->valid = false;
return;
}
bool
Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
struct mission_item_s *new_mission_item)
{
/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
for (int i=0; i<10; i++) {
const ssize_t len = sizeof(struct mission_item_s);
/* read mission item from datamanager */
if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR waypoint could not be read");
return false;
}
/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
/* do DO_JUMP as many times as requested */
if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
/* only raise the repeat count if this is for the current mission item
* but not for the next mission item */
if (is_current) {
(new_mission_item->do_jump_current_count)++;
/* save repeat count */
if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP waypoint could not be written");
return false;
}
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
*mission_index = new_mission_item->do_jump_mission_index;
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: DO JUMP repetitions completed");
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index)++;
}
} else {
/* if it's not a DO_JUMP, then we were successful */
return true;
}
}
/* we have given up, we don't want to cycle forever */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP is cycling, giving up");
return false;
}
void
Mission::report_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
}
publish_mission_result();
}
void
Mission::report_current_offboard_mission_item()
{
_mission_result.index_current_mission = _current_offboard_mission_index;
publish_mission_result();
}
void
Mission::report_mission_finished()
{
_mission_result.mission_finished = true;
publish_mission_result();
}
void
Mission::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.mission_reached = false;
_mission_result.mission_finished = false;
}
src/modules/navigator/mission.h
+178
View File
@@ -0,0 +1,178 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mission.h
*
* Navigator mode to access missions
*
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_MISSION_H
#define NAVIGATOR_MISSION_H
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <dataman/dataman.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include "navigator_mode.h"
#include "mission_block.h"
#include "mission_feasibility_checker.h"
class Navigator;
class Mission : public MissionBlock
{
public:
/**
* Constructor
*/
Mission(Navigator *navigator, const char *name);
/**
* Destructor
*/
virtual ~Mission();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called while the mode is active
*/
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
private:
/**
* Update onboard mission topic
*/
void update_onboard_mission();
/**
* Update offboard mission topic
*/
void update_offboard_mission();
/**
* Move on to next mission item or switch to loiter
*/
void advance_mission();
/**
* Set new mission items
*/
void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* Try to set the current position setpoint from an onboard mission item
* @return true if mission item successfully set
*/
bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the current position setpoint from an offboard mission item
* @return true if mission item successfully set
*/
bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the next position setpoint from an onboard mission item
*/
void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Try to set the next position setpoint from an offboard mission item
*/
void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Read a mission item from the dataman and watch out for DO_JUMPS
* @return true if successful
*/
bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
struct mission_item_s *new_mission_item);
/**
* Report that a mission item has been reached
*/
void report_mission_item_reached();
/**
* Rport the current mission item
*/
void report_current_offboard_mission_item();
/**
* Report that the mission is finished if one exists or that none exists
*/
void report_mission_finished();
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
control::BlockParamFloat _param_onboard_enabled;
struct mission_s _onboard_mission;
struct mission_s _offboard_mission;
int _current_onboard_mission_index;
int _current_offboard_mission_index;
orb_advert_t _mission_result_pub;
struct mission_result_s _mission_result;
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_ONBOARD,
MISSION_TYPE_OFFBOARD
} _mission_type;
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
};
#endif
src/modules/navigator/mission_block.cpp
+244
View File
@@ -0,0 +1,244 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mission_block.cpp
*
* Helper class to use mission items
*
* @author Julian Oes <julian@oes.ch>
*/
#include <sys/types.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <float.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include "navigator.h"
#include "mission_block.h"
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_mission_item({0}),
_mission_item_valid(false)
{
}
MissionBlock::~MissionBlock()
{
}
bool
MissionBlock::is_mission_item_reached()
{
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return _navigator->get_vstatus()->condition_landed;
}
/* TODO: count turns */
if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
return false;
}
hrt_abstime now = hrt_absolute_time();
if (!_waypoint_position_reached) {
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
float altitude_amsl = _mission_item.altitude_is_relative
? _mission_item.altitude + _navigator->get_home_position()->alt
: _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */
if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else {
/* for normal mission items used their acceptance radius */
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
_waypoint_position_reached = true;
}
}
}
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
_waypoint_yaw_reached = true;
}
} else {
_waypoint_yaw_reached = true;
}
}
/* check if the current waypoint was reached */
if (_waypoint_position_reached && _waypoint_yaw_reached) {
if (_time_first_inside_orbit == 0) {
_time_first_inside_orbit = now;
// if (_mission_item.time_inside > 0.01f) {
// mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
// (double)_mission_item.time_inside);
// }
}
/* check if the MAV was long enough inside the waypoint orbit */
if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
return true;
}
}
return false;
}
void
MissionBlock::reset_mission_item_reached()
{
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
_time_first_inside_orbit = 0;
}
void
MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
{
sp->valid = true;
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
switch (item->nav_cmd) {
case NAV_CMD_IDLE:
sp->type = SETPOINT_TYPE_IDLE;
break;
case NAV_CMD_TAKEOFF:
sp->type = SETPOINT_TYPE_TAKEOFF;
break;
case NAV_CMD_LAND:
sp->type = SETPOINT_TYPE_LAND;
break;
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = SETPOINT_TYPE_LOITER;
break;
default:
sp->type = SETPOINT_TYPE_POSITION;
break;
}
}
void
MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* reuse current setpoint as previous setpoint */
if (pos_sp_triplet->current.valid) {
memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
}
}
bool
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* don't change setpoint if 'can_loiter_at_sp' flag set */
if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
/* use current position */
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
_navigator->set_can_loiter_at_sp(true);
}
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|| (fabsf(pos_sp_triplet->current.loiter_radius - _navigator->get_loiter_radius()) > FLT_EPSILON)
|| pos_sp_triplet->current.loiter_direction != 1
|| pos_sp_triplet->previous.valid
|| !pos_sp_triplet->current.valid
|| pos_sp_triplet->next.valid) {
/* position setpoint triplet should be updated */
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
pos_sp_triplet->current.loiter_direction = 1;
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
return true;
}
return false;
}
src/modules/navigator/navigator_mission.h → src/modules/navigator/mission_block.h
+50 -49
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,75 +31,76 @@
*
****************************************************************************/
/**
* @file navigator_mission.h
* Helper class to access missions
* @file mission_block.h
*
* @author Julian Oes <joes@student.ethz.ch>
* Helper class to use mission items
*
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_MISSION_H
#define NAVIGATOR_MISSION_H
#ifndef NAVIGATOR_MISSION_BLOCK_H
#define NAVIGATOR_MISSION_BLOCK_H
#include <drivers/drv_hrt.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include "navigator_mode.h"
class __EXPORT Mission
class Navigator;
class MissionBlock : public NavigatorMode
{
public:
/**
* Constructor
*/
Mission();
MissionBlock(Navigator *navigator, const char *name);
/**
* Destructor, also kills the sensors task.
* Destructor
*/
~Mission();
virtual ~MissionBlock();
void set_offboard_dataman_id(int new_id);
void set_current_offboard_mission_index(int new_index);
void set_current_onboard_mission_index(int new_index);
void set_offboard_mission_count(unsigned new_count);
void set_onboard_mission_count(unsigned new_count);
/**
* Check if mission item has been reached
* @return true if successfully reached
*/
bool is_mission_item_reached();
/**
* Reset all reached flags
*/
void reset_mission_item_reached();
void set_onboard_mission_allowed(bool allowed);
/**
* Convert a mission item to a position setpoint
*
* @param the mission item to convert
* @param the position setpoint that needs to be set
*/
void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
bool current_mission_available();
bool next_mission_available();
/**
* Set previous position setpoint to current setpoint
*/
void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index);
int get_next_mission_item(struct mission_item_s *mission_item);
/**
* Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
*
* @param the position setpoint triplet to set
* @return true if setpoint has changed
*/
bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
void move_to_next();
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
hrt_abstime _time_first_inside_orbit;
void report_mission_item_reached();
void report_current_offboard_mission_item();
void publish_mission_result();
private:
bool current_onboard_mission_available();
bool current_offboard_mission_available();
bool next_onboard_mission_available();
bool next_offboard_mission_available();
int _offboard_dataman_id;
unsigned _current_offboard_mission_index;
unsigned _current_onboard_mission_index;
unsigned _offboard_mission_item_count; /** number of offboard mission items available */
unsigned _onboard_mission_item_count; /** number of onboard mission items available */
bool _onboard_mission_allowed;
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_ONBOARD,
MISSION_TYPE_OFFBOARD,
} _current_mission_type;
int _mission_result_pub;
struct mission_result_s _mission_result;
mission_item_s _mission_item;
bool _mission_item_valid;
};
#endif
src/modules/navigator/mission_feasibility_checker.cpp
+2 -1
View File
@@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
return false;
}
void MissionFeasibilityChecker::updateNavigationCapabilities()
{
int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
}
void MissionFeasibilityChecker::init()
src/modules/navigator/mission_params.c
+69
View File
@@ -0,0 +1,69 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mission_params.c
*
* Parameters for mission.
*
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Mission parameters, accessible via MAVLink
*/
/**
* Take-off altitude
*
* Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to
* MIS_TAKEOFF_ALT on takeoff, then go to waypoint.
*
* @unit meters
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
/**
* Enable onboard mission
*
* @min 0
* @max 1
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
src/modules/navigator/module.mk
+8 -2
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -39,7 +39,13 @@ MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c \
navigator_mission.cpp \
navigator_mode.cpp \
mission_block.cpp \
mission.cpp \
mission_params.c \
loiter.cpp \
rtl.cpp \
rtl_params.c \
mission_feasibility_checker.cpp \
geofence.cpp \
geofence_params.c
src/modules/navigator/navigator.h
+219
View File
@@ -0,0 +1,219 @@
/***************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator.h
* Helper class to access missions
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_H
#define NAVIGATOR_H
#include <systemlib/perf_counter.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include "navigator_mode.h"
#include "mission.h"
#include "loiter.h"
#include "rtl.h"
#include "geofence.h"
/**
* Number of navigation modes that need on_active/on_inactive calls
* Currently: mission, loiter, and rtl
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 3
class Navigator : public control::SuperBlock
{
public:
/**
* Constructor
*/
Navigator();
/**
* Destructor, also kills the navigators task.
*/
~Navigator();
/**
* Start the navigator task.
*
* @return OK on success.
*/
int start();
/**
* Display the navigator status.
*/
void status();
/**
* Add point to geofence
*/
void add_fence_point(int argc, char *argv[]);
/**
* Load fence from file
*/
void load_fence_from_file(const char *filename);
/**
* Setters
*/
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
/**
* Getters
*/
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct home_position_s* get_home_position() { return &_home_pos; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
Geofence& get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
float get_acceptance_radius() { return _param_acceptance_radius.get(); }
int get_mavlink_fd() { return _mavlink_fd; }
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _navigator_task; /**< task handle for sensor task */
int _mavlink_fd; /**< the file descriptor to send messages over mavlink */
int _global_pos_sub; /**< global position subscription */
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
int _onboard_mission_sub; /**< onboard mission subscription */
int _offboard_mission_sub; /**< offboard mission subscription */
int _param_update_sub; /**< param update subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
vehicle_global_position_s _global_pos; /**< global vehicle position */
home_position_s _home_pos; /**< home position for RTL */
mission_item_s _mission_item; /**< current mission item */
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
bool _mission_item_valid; /**< flags if the current mission item is valid */
perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence; /**< class that handles the geofence */
bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */
Mission _mission; /**< class that handles the missions */
Loiter _loiter; /**< class that handles loiter */
RTL _rtl; /**< class that handles RTL */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _update_triplet; /**< flags if position SP triplet needs to be published */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
/**
* Retrieve global position
*/
void global_position_update();
/**
* Retrieve home position
*/
void home_position_update();
/**
* Retreive navigation capabilities
*/
void navigation_capabilities_update();
/**
* Retrieve vehicle status
*/
void vehicle_status_update();
/**
* Retrieve vehicle control mode
*/
void vehicle_control_mode_update();
/**
* Update parameters
*/
void params_update();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main task.
*/
void task_main();
/**
* Translate mission item to a position setpoint.
*/
void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
/**
* Publish a new position setpoint triplet for position controllers
*/
void publish_position_setpoint_triplet();
};
#endif
src/modules/navigator/navigator_main.cpp
+138 -1304
View File
File diff suppressed because it is too large Load Diff
src/modules/navigator/navigator_mission.cpp
-319
View File
@@ -1,319 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file navigator_mission.cpp
* Helper class to access missions
*
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <string.h>
#include <stdlib.h>
#include <dataman/dataman.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission_result.h>
#include "navigator_mission.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Mission::Mission() :
_offboard_dataman_id(-1),
_current_offboard_mission_index(0),
_current_onboard_mission_index(0),
_offboard_mission_item_count(0),
_onboard_mission_item_count(0),
_onboard_mission_allowed(false),
_current_mission_type(MISSION_TYPE_NONE),
_mission_result_pub(-1)
{
memset(&_mission_result, 0, sizeof(struct mission_result_s));
}
Mission::~Mission()
{
}
void
Mission::set_offboard_dataman_id(int new_id)
{
_offboard_dataman_id = new_id;
}
void
Mission::set_current_offboard_mission_index(int new_index)
{
if (new_index != -1) {
warnx("specifically set to %d", new_index);
_current_offboard_mission_index = (unsigned)new_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_offboard_mission_index >= _offboard_mission_item_count) {
_current_offboard_mission_index = 0;
}
}
report_current_offboard_mission_item();
}
void
Mission::set_current_onboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_onboard_mission_index = (unsigned)new_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_onboard_mission_index >= _onboard_mission_item_count) {
_current_onboard_mission_index = 0;
}
}
// TODO: implement this for onboard missions as well
// report_current_mission_item();
}
void
Mission::set_offboard_mission_count(unsigned new_count)
{
_offboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_count(unsigned new_count)
{
_onboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_allowed(bool allowed)
{
_onboard_mission_allowed = allowed;
}
bool
Mission::current_mission_available()
{
return (current_onboard_mission_available() || current_offboard_mission_available());
}
bool
Mission::next_mission_available()
{
return (next_onboard_mission_available() || next_offboard_mission_available());
}
int
Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
{
/* try onboard mission first */
if (current_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
_current_mission_type = MISSION_TYPE_ONBOARD;
*onboard = true;
*index = _current_onboard_mission_index;
/* otherwise fallback to offboard */
} else if (current_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
}
_current_mission_type = MISSION_TYPE_OFFBOARD;
*onboard = false;
*index = _current_offboard_mission_index;
} else {
/* happens when no more mission items can be added as a next item */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
}
return OK;
}
int
Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
{
/* try onboard mission first */
if (next_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
/* otherwise fallback to offboard */
} else if (next_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
} else {
/* happens when no more mission items can be added as a next item */
return ERROR;
}
return OK;
}
bool
Mission::current_onboard_mission_available()
{
return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
}
bool
Mission::current_offboard_mission_available()
{
return _offboard_mission_item_count > _current_offboard_mission_index;
}
bool
Mission::next_onboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_ONBOARD) {
next = 1;
}
return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
}
bool
Mission::next_offboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
next = 1;
}
return _offboard_mission_item_count > (_current_offboard_mission_index + next);
}
void
Mission::move_to_next()
{
switch (_current_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
}
}
void
Mission::report_mission_item_reached()
{
if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
}
}
void
Mission::report_current_offboard_mission_item()
{
_mission_result.index_current_mission = _current_offboard_mission_index;
}
void
Mission::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.mission_reached = false;
}
src/modules/navigator/navigator_mode.cpp
+70
View File
@@ -0,0 +1,70 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mode.cpp
*
* Helper class for different modes in navigator
*
* @author Julian Oes <julian@oes.ch>
*/
#include "navigator_mode.h"
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
SuperBlock(NULL, name),
_navigator(navigator),
_first_run(true)
{
/* load initial params */
updateParams();
/* set initial mission items */
on_inactive();
}
NavigatorMode::~NavigatorMode()
{
}
void
NavigatorMode::on_inactive()
{
_first_run = true;
}
bool
NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
pos_sp_triplet->current.valid = false;
_first_run = false;
return false;
}
src/modules/navigator/navigator_mode.h
+86
View File
@@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mode.h
*
* Helper class for different modes in navigator
*
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_MODE_H
#define NAVIGATOR_MODE_H
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <dataman/dataman.h>
#include <uORB/topics/position_setpoint_triplet.h>
class Navigator;
class NavigatorMode : public control::SuperBlock
{
public:
/**
* Constructor
*/
NavigatorMode(Navigator *navigator, const char *name);
/**
* Destructor
*/
virtual ~NavigatorMode();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called while the mode is active
*
* @param position setpoint triplet to set
* @return true if position setpoint triplet has been changed
*/
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
protected:
Navigator *_navigator;
bool _first_run;
};
#endif
src/modules/navigator/navigator_params.c
+12 -83
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -17,7 +17,7 @@
* 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
* 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,
@@ -34,104 +34,33 @@
/**
* @file navigator_params.c
*
* Parameters defined by the navigator task.
* Parameters for navigator in general
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Navigator parameters, accessible via MAVLink
*/
/**
* Minimum altitude (fixed wing only)
* Loiter radius (FW only)
*
* Minimum altitude above home for LOITER.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
/**
* Waypoint acceptance radius
*
* Default value of acceptance radius (if not specified in mission item).
* Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
*
* @unit meters
* @min 0.0
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
/**
* Loiter radius (fixed wing only)
*
* Default value of loiter radius (if not specified in mission item).
*
* @unit meters
* @min 0.0
* @group Navigation
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Enable onboard mission
* Acceptance Radius
*
* @group Navigation
*/
PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
/**
* Take-off altitude
*
* Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
* Default acceptance radius, overridden by acceptance radius of waypoint if set.
*
* @unit meters
* @group Navigation
* @min 1.0
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
/**
* Landing altitude
*
* Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
/**
* Return-To-Launch altitude
*
* Minimum altitude above home position for going home in RTL mode.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
/**
* Return-To-Launch delay
*
* Delay after descend before landing in RTL mode.
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
*
* @unit seconds
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
/**
* Enable parachute deployment
*
* @group Navigation
*/
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
src/modules/navigator/rtl.cpp
+320
View File
@@ -0,0 +1,320 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_rtl.cpp
* Helper class to access RTL
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include "navigator.h"
#include "rtl.h"
#define DELAY_SIGMA 0.01f
RTL::RTL(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY")
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
RTL::~RTL()
{
}
void
RTL::on_inactive()
{
_first_run = true;
/* reset RTL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_rtl_state = RTL_STATE_NONE;
}
}
bool
RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
bool updated = false;
if (_first_run) {
_first_run = false;
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item(pos_sp_triplet);
updated = true;
} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item(pos_sp_triplet);
updated = true;
}
return updated;
}
void
RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
{
/* make sure we have the latest params */
updateParams();
set_previous_pos_setpoint(pos_sp_triplet);
_navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_RETURN: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
if (pos_sp_triplet->previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
_mission_item.yaw = get_bearing_to_next_waypoint(
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
_mission_item.lat, _mission_item.lon);
} else {
/* else use current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_DESCEND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_LOITER: {
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
if (autoland) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside);
} else {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
}
break;
}
case RTL_STATE_LAND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
break;
}
case RTL_STATE_LANDED: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_IDLE;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
break;
}
default:
break;
}
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
reset_mission_item_reached();
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
}
void
RTL::advance_rtl()
{
switch (_rtl_state) {
case RTL_STATE_CLIMB:
_rtl_state = RTL_STATE_RETURN;
break;
case RTL_STATE_RETURN:
_rtl_state = RTL_STATE_DESCEND;
break;
case RTL_STATE_DESCEND:
/* only go to land if autoland is enabled */
if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
_rtl_state = RTL_STATE_LOITER;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LOITER:
_rtl_state = RTL_STATE_LAND;
break;
case RTL_STATE_LAND:
_rtl_state = RTL_STATE_LANDED;
break;
default:
break;
}
}
src/modules/navigator/rtl.h
+110
View File
@@ -0,0 +1,110 @@
/***************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_rtl.h
* Helper class for RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_RTL_H
#define NAVIGATOR_RTL_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include "navigator_mode.h"
#include "mission_block.h"
class Navigator;
class RTL : public MissionBlock
{
public:
/**
* Constructor
*/
RTL(Navigator *navigator, const char *name);
/**
* Destructor
*/
~RTL();
/**
* This function is called while the mode is inactive
*/
void on_inactive();
/**
* This function is called while the mode is active
*
* @param position setpoint triplet that needs to be set
* @return true if updated
*/
bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
private:
/**
* Set the RTL item
*/
void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet);
/**
* Move to next RTL item
*/
void advance_rtl();
enum RTLState {
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_DESCEND,
RTL_STATE_LOITER,
RTL_STATE_LAND,
RTL_STATE_LANDED,
} _rtl_state;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
};
#endif
src/modules/navigator/rtl_params.c
+98
View File
@@ -0,0 +1,98 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rtl_params.c
*
* Parameters for RTL
*
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* RTL parameters, accessible via MAVLink
*/
/**
* Loiter radius after RTL (FW only)
*
* Default value of loiter radius after RTL (fixedwing only).
*
* @unit meters
* @min 0.0
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
/**
* RTL altitude
*
* Altitude to fly back in RTL in meters
*
* @unit meters
* @min 0
* @max 1
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
/**
* RTL loiter altitude
*
* Stay at this altitude above home position after RTL descending.
* Land (i.e. slowly descend) from this altitude if autolanding allowed.
*
* @unit meters
* @min 0
* @max 100
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
/**
* RTL delay
*
* Delay after descend before landing in RTL mode.
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
*
* @unit seconds
* @min -1
* @max
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
src/modules/position_estimator_inav/position_estimator_inav_main.c
+35 -33
View File
@@ -49,6 +49,7 @@
#include <sys/prctl.h>
#include <termios.h>
#include <math.h>
#include <float.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_controls.h>
@@ -179,15 +180,21 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1);
}
void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
{
FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) {
char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n",
hrt_absolute_time(), msg, (double)dt,
(double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1],
(double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]);
fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n",
(double)acc[0], (double)acc[1], (double)acc[2],
(double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1],
(double)w_xy_gps_p, (double)w_xy_gps_v);
fwrite(s, 1, n, f);
free(s);
}
@@ -261,9 +268,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
hrt_abstime t_prev = 0;
/* 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 acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
@@ -285,7 +289,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime xy_src_time = 0; // time of last available position data
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
@@ -370,8 +373,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
warnx("baro offs: %.2f", baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
warnx("baro offs: %.2f", (double)baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
@@ -475,7 +478,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
flow_prev = flow.flow_timestamp;
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
(att.R[2][2] > 0.7f) &&
(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
@@ -497,7 +504,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset);
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
}
} else {
@@ -510,7 +517,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) {
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
/* distance to surface */
float flow_dist = dist_bottom / att.R[2][2];
/* check if flow if too large for accurate measurements */
@@ -558,7 +565,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* under ideal conditions, on 1m distance assume EPH = 10cm */
eph_flow = 0.1 / w_flow;
eph_flow = 0.1f / w_flow;
flow_valid = true;
@@ -619,13 +626,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) {
if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) {
if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
gps_valid = true;
reset_est = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
@@ -658,8 +665,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* initialize projection */
map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", lat, lon, alt);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
}
}
@@ -702,8 +709,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* save rotation matrix at this moment */
memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
}
} else {
@@ -743,10 +750,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* increase EPH/EPV on each step */
if (eph < max_eph_epv) {
eph *= 1.0 + dt;
eph *= 1.0f + dt;
}
if (epv < max_eph_epv) {
epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift)
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
}
/* use GPS if it's valid and reference position initialized */
@@ -755,11 +762,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
/* try to estimate position during some time after position sources lost */
if (use_gps_xy || use_flow) {
xy_src_time = t;
}
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
@@ -856,7 +858,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
if (use_gps_z) {
epv = fminf(epv, gps.epv_m);
epv = fminf(epv, gps.epv);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
}
@@ -891,7 +893,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (use_gps_xy) {
eph = fminf(eph, gps.eph_m);
eph = fminf(eph, gps.eph);
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
@@ -952,11 +954,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
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);
(double)(accel_updates / updates_dt),
(double)(baro_updates / updates_dt),
(double)(gps_updates / updates_dt),
(double)(attitude_updates / updates_dt),
(double)(flow_updates / updates_dt));
updates_counter_start = t;
accel_updates = 0;
baro_updates = 0;
src/modules/px4iofirmware/i2c.c
+2
View File
@@ -331,6 +331,7 @@ i2c_tx_complete(void)
i2c_tx_setup();
}
#ifdef DEBUG
static void
i2c_dump(void)
{
@@ -339,3 +340,4 @@ i2c_dump(void)
debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE);
debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2);
}
#endif
src/modules/px4iofirmware/protocol.h
+1 -1
View File
@@ -312,7 +312,7 @@ struct IOPacket {
#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK)
#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))))
static const uint8_t crc8_tab[256] __attribute__((unused)) =
{
src/modules/px4iofirmware/px4io.c
+2 -3
View File
@@ -37,6 +37,7 @@
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <stdio.h> // required for task_create
#include <stdbool.h>
@@ -303,14 +304,12 @@ user_start(int argc, char *argv[])
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
struct mallinfo minfo = mallinfo();
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)minfo.mxordblk);
(unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
}

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