forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/navigator_new' into fw_autoland_att_tecs_navigator_termination
This commit is contained in:
Thomas Gubler
commit
4f5791d4b1
|
|
@ -8,30 +8,35 @@ echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV"
|
|||
if param compare SYS_AUTOCONFIG 1
|
||||
then
|
||||
# Set all params here, then disable autoconfig
|
||||
param set FW_AIRSPD_MIN 11.4
|
||||
param set FW_AIRSPD_TRIM 14
|
||||
param set FW_AIRSPD_MAX 22
|
||||
param set FW_L1_PERIOD 15
|
||||
param set FW_P_D 0
|
||||
param set FW_P_I 0
|
||||
param set FW_P_IMAX 15
|
||||
param set FW_P_LIM_MAX 50
|
||||
param set FW_P_LIM_MIN -50
|
||||
param set FW_P_LIM_MAX 45
|
||||
param set FW_P_LIM_MIN -45
|
||||
param set FW_P_P 60
|
||||
param set FW_P_RMAX_NEG 0
|
||||
param set FW_P_RMAX_POS 0
|
||||
param set FW_P_ROLLFF 1.1
|
||||
param set FW_P_ROLLFF 2
|
||||
param set FW_R_D 0
|
||||
param set FW_R_I 5
|
||||
param set FW_R_IMAX 20
|
||||
param set FW_R_P 100
|
||||
param set FW_R_RMAX 100
|
||||
param set FW_THR_CRUISE 0.65
|
||||
param set FW_R_IMAX 15
|
||||
param set FW_R_P 80
|
||||
param set FW_R_RMAX 60
|
||||
param set FW_THR_CRUISE 0.8
|
||||
param set FW_THR_LND_MAX 0
|
||||
param set FW_THR_MAX 1
|
||||
param set FW_THR_MIN 0
|
||||
param set FW_THR_MIN 0.5
|
||||
param set FW_T_SINK_MAX 5.0
|
||||
param set FW_T_SINK_MIN 4.0
|
||||
param set FW_Y_ROLLFF 1.1
|
||||
param set FW_L1_PERIOD 17
|
||||
param set RC_SCALE_ROLL 1.0
|
||||
param set RC_SCALE_PITCH 1.0
|
||||
|
||||
param set FW_T_SINK_MIN 2.0
|
||||
param set FW_Y_ROLLFF 1.0
|
||||
param set RC_SCALE_ROLL 0.6
|
||||
param set RC_SCALE_PITCH 0.6
|
||||
param set TRIM_PITCH 0.1
|
||||
|
||||
param set SYS_AUTOCONFIG 0
|
||||
param save
|
||||
fi
|
||||
|
|
|
|||
|
|
@ -0,0 +1,91 @@
|
|||
#!nsh
|
||||
|
||||
echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)"
|
||||
|
||||
#
|
||||
# Load default params for this platform
|
||||
#
|
||||
if param compare SYS_AUTOCONFIG 1
|
||||
then
|
||||
# Set all params here, then disable autoconfig
|
||||
param set FW_AIRSPD_MIN 7
|
||||
param set FW_AIRSPD_TRIM 9
|
||||
param set FW_AIRSPD_MAX 14
|
||||
param set FW_L1_PERIOD 10
|
||||
param set FW_P_D 0
|
||||
param set FW_P_I 0
|
||||
param set FW_P_IMAX 20
|
||||
param set FW_P_LIM_MAX 30
|
||||
param set FW_P_LIM_MIN -20
|
||||
param set FW_P_P 30
|
||||
param set FW_P_RMAX_NEG 0
|
||||
param set FW_P_RMAX_POS 0
|
||||
param set FW_P_ROLLFF 2
|
||||
param set FW_R_D 0
|
||||
param set FW_R_I 5
|
||||
param set FW_R_IMAX 20
|
||||
param set FW_R_P 60
|
||||
param set FW_R_RMAX 60
|
||||
param set FW_THR_CRUISE 0.65
|
||||
param set FW_THR_MAX 0.7
|
||||
param set FW_THR_MIN 0
|
||||
param set FW_T_SINK_MAX 5
|
||||
param set FW_T_SINK_MIN 2
|
||||
param set FW_T_TIME_CONST 9
|
||||
param set FW_Y_ROLLFF 2.0
|
||||
param set RC_SCALE_ROLL 1.0
|
||||
param set RC_SCALE_PITCH 1.0
|
||||
|
||||
param set SYS_AUTOCONFIG 0
|
||||
param save
|
||||
fi
|
||||
|
||||
#
|
||||
# Force some key parameters to sane values
|
||||
# MAV_TYPE 1 = fixed wing
|
||||
#
|
||||
param set MAV_TYPE 1
|
||||
|
||||
set EXIT_ON_END no
|
||||
|
||||
#
|
||||
# Start and configure PX4IO or FMU interface
|
||||
#
|
||||
if px4io detect
|
||||
then
|
||||
# Start MAVLink (depends on orb)
|
||||
mavlink start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
|
||||
px4io limit 100
|
||||
else
|
||||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
||||
#
|
||||
# Load mixer and start controllers (depends on px4io)
|
||||
#
|
||||
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
|
||||
then
|
||||
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
|
||||
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
|
||||
else
|
||||
echo "Using /etc/mixers/FMU_Q.mix"
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
|
||||
fi
|
||||
|
||||
#
|
||||
# Start common fixedwing apps
|
||||
#
|
||||
sh /etc/init.d/rc.fixedwing
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
exit
|
||||
fi
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
#!nsh
|
||||
|
||||
echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo"
|
||||
|
||||
#
|
||||
# Load default params for this platform
|
||||
#
|
||||
if param compare SYS_AUTOCONFIG 1
|
||||
then
|
||||
# Set all params here, then disable autoconfig
|
||||
param set FW_AIRSPD_MAX 20
|
||||
param set FW_AIRSPD_TRIM 12
|
||||
param set FW_AIRSPD_MIN 15
|
||||
param set FW_L1_PERIOD 12
|
||||
param set FW_P_D 0
|
||||
param set FW_P_I 0
|
||||
param set FW_P_IMAX 15
|
||||
param set FW_P_LIM_MAX 50
|
||||
param set FW_P_LIM_MIN -50
|
||||
param set FW_P_P 60
|
||||
param set FW_P_RMAX_NEG 0
|
||||
param set FW_P_RMAX_POS 0
|
||||
param set FW_P_ROLLFF 1.1
|
||||
param set FW_R_D 0
|
||||
param set FW_R_I 5
|
||||
param set FW_R_IMAX 20
|
||||
param set FW_R_P 80
|
||||
param set FW_R_RMAX 100
|
||||
param set FW_THR_CRUISE 0.75
|
||||
param set FW_THR_MAX 1
|
||||
param set FW_THR_MIN 0
|
||||
param set FW_T_SINK_MAX 5.0
|
||||
param set FW_T_SINK_MIN 4.0
|
||||
param set FW_T_TIME_CONST 9
|
||||
param set FW_Y_ROLLFF 1.1
|
||||
param set RC_SCALE_ROLL 1.0
|
||||
param set RC_SCALE_PITCH 1.0
|
||||
|
||||
param set SYS_AUTOCONFIG 0
|
||||
param save
|
||||
fi
|
||||
|
||||
#
|
||||
# Force some key parameters to sane values
|
||||
# MAV_TYPE 1 = fixed wing
|
||||
#
|
||||
param set MAV_TYPE 1
|
||||
|
||||
set EXIT_ON_END no
|
||||
|
||||
#
|
||||
# Start and configure PX4IO or FMU interface
|
||||
#
|
||||
if px4io detect
|
||||
then
|
||||
# Start MAVLink (depends on orb)
|
||||
mavlink start
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
|
||||
px4io limit 100
|
||||
else
|
||||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS0
|
||||
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
||||
#
|
||||
# Load mixer and start controllers (depends on px4io)
|
||||
#
|
||||
if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ]
|
||||
then
|
||||
echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix"
|
||||
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix
|
||||
else
|
||||
echo "Using /etc/mixers/FMU_FX79.mix"
|
||||
mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix
|
||||
fi
|
||||
|
||||
#
|
||||
# Start common fixedwing apps
|
||||
#
|
||||
sh /etc/init.d/rc.fixedwing
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
exit
|
||||
fi
|
||||
|
|
@ -331,6 +331,18 @@ then
|
|||
set MODE custom
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 33
|
||||
then
|
||||
sh /etc/init.d/33_io_wingwing
|
||||
set MODE custom
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 34
|
||||
then
|
||||
sh /etc/init.d/34_io_fx79
|
||||
set MODE custom
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 40
|
||||
then
|
||||
sh /etc/init.d/40_io_segway
|
||||
|
|
|
|||
|
|
@ -0,0 +1,72 @@
|
|||
FX-79 Delta-wing mixer for PX4FMU
|
||||
=================================
|
||||
|
||||
Designed for FX-79.
|
||||
|
||||
TODO (sjwilks): Add mixers for flaps.
|
||||
|
||||
This file defines mixers suitable for controlling a delta wing aircraft using
|
||||
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
|
||||
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
|
||||
assumed to be unused.
|
||||
|
||||
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
|
||||
(roll), 1 (pitch) and 3 (thrust).
|
||||
|
||||
See the README for more information on the scaler format.
|
||||
|
||||
Elevon mixers
|
||||
-------------
|
||||
Three scalers total (output, roll, pitch).
|
||||
|
||||
On the assumption that the two elevon servos are physically reversed, the pitch
|
||||
input is inverted between the two servos.
|
||||
|
||||
The scaling factor for roll inputs is adjusted to implement differential travel
|
||||
for the elevons.
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 5000 8000 0 -10000 10000
|
||||
S: 0 1 8000 8000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 8000 5000 0 -10000 10000
|
||||
S: 0 1 -8000 -8000 0 -10000 10000
|
||||
|
||||
Output 2
|
||||
--------
|
||||
This mixer is empty.
|
||||
|
||||
Z:
|
||||
|
||||
Motor speed mixer
|
||||
-----------------
|
||||
Two scalers total (output, thrust).
|
||||
|
||||
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
|
||||
range. Inputs below zero are treated as zero.
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
Gimbal / flaps / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
@ -850,7 +850,7 @@ PX4IO::task_main()
|
|||
|
||||
/* we're not nice to the lower-priority control groups and only check them
|
||||
when the primary group updated (which is now). */
|
||||
io_set_control_groups();
|
||||
(void)io_set_control_groups();
|
||||
}
|
||||
|
||||
if (now >= poll_last + IO_POLL_INTERVAL) {
|
||||
|
|
@ -962,14 +962,14 @@ out:
|
|||
int
|
||||
PX4IO::io_set_control_groups()
|
||||
{
|
||||
bool attitude_ok = io_set_control_state(0);
|
||||
int ret = io_set_control_state(0);
|
||||
|
||||
/* send auxiliary control groups */
|
||||
(void)io_set_control_state(1);
|
||||
(void)io_set_control_state(2);
|
||||
(void)io_set_control_state(3);
|
||||
|
||||
return attitude_ok;
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -1095,8 +1095,10 @@ PX4IO::io_set_rc_config()
|
|||
* assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical
|
||||
* controls.
|
||||
*/
|
||||
|
||||
/* fill the mapping with an error condition triggering value */
|
||||
for (unsigned i = 0; i < _max_rc_input; i++)
|
||||
input_map[i] = -1;
|
||||
input_map[i] = UINT8_MAX;
|
||||
|
||||
/*
|
||||
* NOTE: The indices for mapped channels are 1-based
|
||||
|
|
@ -1128,12 +1130,6 @@ PX4IO::io_set_rc_config()
|
|||
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
|
||||
input_map[ichan - 1] = 4;
|
||||
|
||||
ichan = 5;
|
||||
|
||||
for (unsigned i = 0; i < _max_rc_input; i++)
|
||||
if (input_map[i] == -1)
|
||||
input_map[i] = ichan++;
|
||||
|
||||
/*
|
||||
* Iterate all possible RC inputs.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -168,7 +168,7 @@
|
|||
# error HRT_TIMER_CLOCK must be greater than 1MHz
|
||||
#endif
|
||||
|
||||
/*
|
||||
/**
|
||||
* Minimum/maximum deadlines.
|
||||
*
|
||||
* These are suitable for use with a 16-bit timer/counter clocked
|
||||
|
|
@ -276,7 +276,7 @@ static void hrt_call_invoke(void);
|
|||
* Specific registers and bits used by PPM sub-functions
|
||||
*/
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
/*
|
||||
/*
|
||||
* If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
|
||||
*
|
||||
* Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
|
||||
|
|
@ -336,17 +336,18 @@ static void hrt_call_invoke(void);
|
|||
/*
|
||||
* PPM decoder tuning parameters
|
||||
*/
|
||||
# define PPM_MAX_PULSE_WIDTH 700 /* maximum width of a valid pulse */
|
||||
# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */
|
||||
# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */
|
||||
# define PPM_MIN_START 2400 /* shortest valid start gap (only 2nd part of pulse) */
|
||||
# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */
|
||||
# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */
|
||||
# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */
|
||||
# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */
|
||||
# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */
|
||||
|
||||
/* decoded PPM buffer */
|
||||
#define PPM_MIN_CHANNELS 5
|
||||
#define PPM_MAX_CHANNELS 20
|
||||
|
||||
/* Number of same-sized frames required to 'lock' */
|
||||
#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */
|
||||
/** Number of same-sized frames required to 'lock' */
|
||||
#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */
|
||||
|
||||
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
|
||||
__EXPORT uint16_t ppm_frame_length = 0;
|
||||
|
|
@ -363,12 +364,12 @@ unsigned ppm_pulse_next;
|
|||
|
||||
static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
|
||||
|
||||
/* PPM decoder state machine */
|
||||
/** PPM decoder state machine */
|
||||
struct {
|
||||
uint16_t last_edge; /* last capture time */
|
||||
uint16_t last_mark; /* last significant edge */
|
||||
uint16_t frame_start; /* the frame width */
|
||||
unsigned next_channel; /* next channel index */
|
||||
uint16_t last_edge; /**< last capture time */
|
||||
uint16_t last_mark; /**< last significant edge */
|
||||
uint16_t frame_start; /**< the frame width */
|
||||
unsigned next_channel; /**< next channel index */
|
||||
enum {
|
||||
UNSYNCH = 0,
|
||||
ARM,
|
||||
|
|
@ -390,7 +391,7 @@ static void hrt_ppm_decode(uint32_t status);
|
|||
# define CCER_PPM 0
|
||||
#endif /* HRT_PPM_CHANNEL */
|
||||
|
||||
/*
|
||||
/**
|
||||
* Initialise the timer we are going to use.
|
||||
*
|
||||
* We expect that we'll own one of the reduced-function STM32 general
|
||||
|
|
@ -436,7 +437,7 @@ hrt_tim_init(void)
|
|||
}
|
||||
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
/*
|
||||
/**
|
||||
* Handle the PPM decoder state machine.
|
||||
*/
|
||||
static void
|
||||
|
|
@ -518,8 +519,8 @@ hrt_ppm_decode(uint32_t status)
|
|||
case ARM:
|
||||
|
||||
/* we expect a pulse giving us the first mark */
|
||||
if (width > PPM_MAX_PULSE_WIDTH)
|
||||
goto error; /* pulse was too long */
|
||||
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
|
||||
goto error; /* pulse was too short or too long */
|
||||
|
||||
/* record the mark timing, expect an inactive edge */
|
||||
ppm.last_mark = ppm.last_edge;
|
||||
|
|
@ -533,8 +534,8 @@ hrt_ppm_decode(uint32_t status)
|
|||
case INACTIVE:
|
||||
|
||||
/* we expect a short pulse */
|
||||
if (width > PPM_MAX_PULSE_WIDTH)
|
||||
goto error; /* pulse was too long */
|
||||
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
|
||||
goto error; /* pulse was too short or too long */
|
||||
|
||||
/* this edge is not interesting, but now we are ready for the next mark */
|
||||
ppm.phase = ACTIVE;
|
||||
|
|
@ -576,7 +577,7 @@ error:
|
|||
}
|
||||
#endif /* HRT_PPM_CHANNEL */
|
||||
|
||||
/*
|
||||
/**
|
||||
* Handle the compare interupt by calling the callout dispatcher
|
||||
* and then re-scheduling the next deadline.
|
||||
*/
|
||||
|
|
@ -605,6 +606,7 @@ hrt_tim_isr(int irq, void *context)
|
|||
|
||||
hrt_ppm_decode(status);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* was this a timer tick? */
|
||||
|
|
@ -623,7 +625,7 @@ hrt_tim_isr(int irq, void *context)
|
|||
return OK;
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Fetch a never-wrapping absolute time value in microseconds from
|
||||
* some arbitrary epoch shortly after system start.
|
||||
*/
|
||||
|
|
@ -670,7 +672,7 @@ hrt_absolute_time(void)
|
|||
return abstime;
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Convert a timespec to absolute time
|
||||
*/
|
||||
hrt_abstime
|
||||
|
|
@ -684,7 +686,7 @@ ts_to_abstime(struct timespec *ts)
|
|||
return result;
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Convert absolute time to a timespec.
|
||||
*/
|
||||
void
|
||||
|
|
@ -695,7 +697,7 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
|
|||
ts->tv_nsec = abstime * 1000;
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Compare a time value with the current time.
|
||||
*/
|
||||
hrt_abstime
|
||||
|
|
@ -710,7 +712,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then)
|
|||
return delta;
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Store the absolute time in an interrupt-safe fashion
|
||||
*/
|
||||
hrt_abstime
|
||||
|
|
@ -725,7 +727,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now)
|
|||
return ts;
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Initalise the high-resolution timing module.
|
||||
*/
|
||||
void
|
||||
|
|
@ -740,7 +742,7 @@ hrt_init(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Call callout(arg) after interval has elapsed.
|
||||
*/
|
||||
void
|
||||
|
|
@ -753,7 +755,7 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v
|
|||
arg);
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Call callout(arg) at calltime.
|
||||
*/
|
||||
void
|
||||
|
|
@ -762,7 +764,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v
|
|||
hrt_call_internal(entry, calltime, 0, callout, arg);
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Call callout(arg) every period.
|
||||
*/
|
||||
void
|
||||
|
|
@ -781,13 +783,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
|
|||
irqstate_t flags = irqsave();
|
||||
|
||||
/* if the entry is currently queued, remove it */
|
||||
/* note that we are using a potentially uninitialised
|
||||
entry->link here, but it is safe as sq_rem() doesn't
|
||||
dereference the passed node unless it is found in the
|
||||
list. So we potentially waste a bit of time searching the
|
||||
queue for the uninitialised entry->link but we don't do
|
||||
anything actually unsafe.
|
||||
*/
|
||||
/* note that we are using a potentially uninitialised
|
||||
entry->link here, but it is safe as sq_rem() doesn't
|
||||
dereference the passed node unless it is found in the
|
||||
list. So we potentially waste a bit of time searching the
|
||||
queue for the uninitialised entry->link but we don't do
|
||||
anything actually unsafe.
|
||||
*/
|
||||
if (entry->deadline != 0)
|
||||
sq_rem(&entry->link, &callout_queue);
|
||||
|
||||
|
|
@ -801,7 +803,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
|
|||
irqrestore(flags);
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* If this returns true, the call has been invoked and removed from the callout list.
|
||||
*
|
||||
* Always returns false for repeating callouts.
|
||||
|
|
@ -812,7 +814,7 @@ hrt_called(struct hrt_call *entry)
|
|||
return (entry->deadline == 0);
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Remove the entry from the callout list.
|
||||
*/
|
||||
void
|
||||
|
|
@ -895,17 +897,18 @@ hrt_call_invoke(void)
|
|||
/* if the callout has a non-zero period, it has to be re-entered */
|
||||
if (call->period != 0) {
|
||||
// re-check call->deadline to allow for
|
||||
// callouts to re-schedule themselves
|
||||
// callouts to re-schedule themselves
|
||||
// using hrt_call_delay()
|
||||
if (call->deadline <= now) {
|
||||
call->deadline = deadline + call->period;
|
||||
}
|
||||
|
||||
hrt_call_enter(call);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
/**
|
||||
* Reschedule the next timer interrupt.
|
||||
*
|
||||
* This routine must be called with interrupts disabled.
|
||||
|
|
|
|||
|
|
@ -52,11 +52,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
|
|||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
|
||||
|
||||
/* gyro measurement noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f);
|
||||
/* accelerometer measurement noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
|
||||
/* accel measurement noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
|
||||
/* mag measurement noise */
|
||||
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);
|
||||
|
|
@ -72,10 +74,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
|
|||
h->q3 = param_find("EKF_ATT_V3_Q3");
|
||||
h->q4 = param_find("EKF_ATT_V3_Q4");
|
||||
|
||||
h->r0 = param_find("EKF_ATT_V3_R0");
|
||||
h->r1 = param_find("EKF_ATT_V3_R1");
|
||||
h->r2 = param_find("EKF_ATT_V3_R2");
|
||||
h->r3 = param_find("EKF_ATT_V3_R3");
|
||||
h->r0 = param_find("EKF_ATT_V4_R0");
|
||||
h->r1 = param_find("EKF_ATT_V4_R1");
|
||||
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");
|
||||
|
|
|
|||
|
|
@ -68,7 +68,6 @@
|
|||
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "missionlib.h"
|
||||
#include "mavlink_hil.h"
|
||||
#include "util.h"
|
||||
#include "waypoints.h"
|
||||
|
|
@ -710,25 +709,25 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
|
||||
mavlink_waypoint_eventloop(hrt_absolute_time());
|
||||
|
||||
/* sleep quarter the time */
|
||||
usleep(25000);
|
||||
|
||||
/* check if waypoint has been reached against the last positions */
|
||||
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
|
||||
mavlink_waypoint_eventloop(hrt_absolute_time());
|
||||
|
||||
/* sleep quarter the time */
|
||||
usleep(25000);
|
||||
|
||||
/* send parameters at 20 Hz (if queued for sending) */
|
||||
mavlink_pm_queued_send();
|
||||
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
|
||||
mavlink_waypoint_eventloop(hrt_absolute_time());
|
||||
|
||||
/* sleep quarter the time */
|
||||
usleep(25000);
|
||||
|
||||
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
|
||||
mavlink_waypoint_eventloop(hrt_absolute_time());
|
||||
|
||||
if (baudrate > 57600) {
|
||||
mavlink_pm_queued_send();
|
||||
|
|
|
|||
|
|
@ -79,7 +79,6 @@ __BEGIN_DECLS
|
|||
#include "mavlink_bridge_header.h"
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "missionlib.h"
|
||||
#include "mavlink_hil.h"
|
||||
#include "mavlink_parameters.h"
|
||||
#include "util.h"
|
||||
|
|
@ -844,7 +843,7 @@ receive_thread(void *arg)
|
|||
handle_message(&msg);
|
||||
|
||||
/* handle packet with waypoint component */
|
||||
mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
|
||||
mavlink_wpm_message_handler(&msg);
|
||||
|
||||
/* handle packet with parameter component */
|
||||
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
|
||||
|
|
|
|||
|
|
@ -1,399 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file missionlib.h
|
||||
* MAVLink missionlib components
|
||||
*/
|
||||
|
||||
// XXX trim includes
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <mqueue.h>
|
||||
#include <string.h>
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "geo/geo.h"
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "missionlib.h"
|
||||
#include "mavlink_hil.h"
|
||||
#include "util.h"
|
||||
#include "waypoints.h"
|
||||
#include "mavlink_parameters.h"
|
||||
|
||||
|
||||
|
||||
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
|
||||
static uint64_t loiter_start_time;
|
||||
|
||||
#if 0
|
||||
static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
|
||||
struct vehicle_global_position_setpoint_s *sp);
|
||||
#endif
|
||||
|
||||
int
|
||||
mavlink_missionlib_send_message(mavlink_message_t *msg)
|
||||
{
|
||||
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
|
||||
|
||||
mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int
|
||||
mavlink_missionlib_send_gcs_string(const char *string)
|
||||
{
|
||||
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
|
||||
mavlink_statustext_t statustext;
|
||||
int i = 0;
|
||||
|
||||
while (i < len - 1) {
|
||||
statustext.text[i] = string[i];
|
||||
|
||||
if (string[i] == '\0')
|
||||
break;
|
||||
|
||||
i++;
|
||||
}
|
||||
|
||||
if (i > 1) {
|
||||
/* Enforce null termination */
|
||||
statustext.text[i] = '\0';
|
||||
mavlink_message_t msg;
|
||||
|
||||
mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
|
||||
return mavlink_missionlib_send_message(&msg);
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get system time since boot in microseconds
|
||||
*
|
||||
* @return the system time since boot in microseconds
|
||||
*/
|
||||
uint64_t mavlink_missionlib_get_system_timestamp()
|
||||
{
|
||||
return hrt_absolute_time();
|
||||
}
|
||||
|
||||
#if 0
|
||||
/**
|
||||
* Set special vehicle setpoint fields based on current mission item.
|
||||
*
|
||||
* @return true if the mission item could be interpreted
|
||||
* successfully, it return false on failure.
|
||||
*/
|
||||
bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
|
||||
struct vehicle_global_position_setpoint_s *sp)
|
||||
{
|
||||
switch (command) {
|
||||
case MAV_CMD_NAV_LOITER_UNLIM:
|
||||
sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
|
||||
break;
|
||||
case MAV_CMD_NAV_LOITER_TIME:
|
||||
sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
|
||||
loiter_start_time = hrt_absolute_time();
|
||||
break;
|
||||
// case MAV_CMD_NAV_LOITER_TURNS:
|
||||
// sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT;
|
||||
// break;
|
||||
case MAV_CMD_NAV_WAYPOINT:
|
||||
sp->nav_cmd = NAV_CMD_WAYPOINT;
|
||||
break;
|
||||
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
||||
sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
|
||||
break;
|
||||
case MAV_CMD_NAV_LAND:
|
||||
sp->nav_cmd = NAV_CMD_LAND;
|
||||
break;
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
sp->nav_cmd = NAV_CMD_TAKEOFF;
|
||||
break;
|
||||
default:
|
||||
/* abort */
|
||||
return false;
|
||||
}
|
||||
|
||||
sp->loiter_radius = param3;
|
||||
sp->loiter_direction = (param3 >= 0) ? 1 : -1;
|
||||
|
||||
sp->param1 = param1;
|
||||
sp->param2 = param2;
|
||||
sp->param3 = param3;
|
||||
sp->param4 = param4;
|
||||
|
||||
|
||||
/* define the turn distance */
|
||||
float orbit = 15.0f;
|
||||
|
||||
if (command == (int)MAV_CMD_NAV_WAYPOINT) {
|
||||
|
||||
orbit = param2;
|
||||
|
||||
} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
|
||||
command == (int)MAV_CMD_NAV_LOITER_TIME ||
|
||||
command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
|
||||
|
||||
orbit = param3;
|
||||
} else {
|
||||
|
||||
// XXX set default orbit via param
|
||||
// 15 initialized above
|
||||
}
|
||||
|
||||
sp->turn_distance_xy = orbit;
|
||||
sp->turn_distance_z = orbit;
|
||||
}
|
||||
|
||||
/**
|
||||
* This callback is executed each time a waypoint changes.
|
||||
*
|
||||
* It publishes the vehicle_global_position_setpoint_s or the
|
||||
* vehicle_local_position_setpoint_s topic, depending on the type of waypoint
|
||||
*/
|
||||
void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
|
||||
float param2, float param3, float param4, float param5_lat_x,
|
||||
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
|
||||
{
|
||||
static orb_advert_t global_position_setpoint_pub = -1;
|
||||
// static orb_advert_t global_position_set_triplet_pub = -1;
|
||||
static orb_advert_t local_position_setpoint_pub = -1;
|
||||
static unsigned last_waypoint_index = -1;
|
||||
char buf[50] = {0};
|
||||
|
||||
// XXX include check if WP is supported, jump to next if not
|
||||
|
||||
/* Update controller setpoints */
|
||||
if (frame == (int)MAV_FRAME_GLOBAL) {
|
||||
/* global, absolute waypoint */
|
||||
struct vehicle_global_position_setpoint_s sp;
|
||||
sp.lat = param5_lat_x * 1e7f;
|
||||
sp.lon = param6_lon_y * 1e7f;
|
||||
sp.altitude = param7_alt_z;
|
||||
sp.altitude_is_relative = false;
|
||||
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
|
||||
set_special_fields(param1, param2, param3, param4, command, &sp);
|
||||
|
||||
/* Initialize setpoint publication if necessary */
|
||||
if (global_position_setpoint_pub < 0) {
|
||||
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
|
||||
}
|
||||
|
||||
|
||||
/* fill triplet: previous, current, next waypoint */
|
||||
// struct vehicle_global_position_set_triplet_s triplet;
|
||||
|
||||
/* current waypoint is same as sp */
|
||||
// memcpy(&(triplet.current), &sp, sizeof(sp));
|
||||
|
||||
/*
|
||||
* Check if previous WP (in mission, not in execution order)
|
||||
* is available and identify correct index
|
||||
*/
|
||||
int last_setpoint_index = -1;
|
||||
bool last_setpoint_valid = false;
|
||||
|
||||
if (index > 0) {
|
||||
last_setpoint_index = index - 1;
|
||||
}
|
||||
|
||||
while (last_setpoint_index >= 0) {
|
||||
|
||||
if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL &&
|
||||
(wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
|
||||
wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
|
||||
wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
|
||||
wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
|
||||
last_setpoint_valid = true;
|
||||
break;
|
||||
}
|
||||
|
||||
last_setpoint_index--;
|
||||
}
|
||||
|
||||
/*
|
||||
* Check if next WP (in mission, not in execution order)
|
||||
* is available and identify correct index
|
||||
*/
|
||||
int next_setpoint_index = -1;
|
||||
bool next_setpoint_valid = false;
|
||||
|
||||
/* next waypoint */
|
||||
if (wpm->size > 1) {
|
||||
next_setpoint_index = index + 1;
|
||||
}
|
||||
|
||||
while (next_setpoint_index < wpm->size) {
|
||||
|
||||
if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
|
||||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
|
||||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
|
||||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
|
||||
next_setpoint_valid = true;
|
||||
break;
|
||||
}
|
||||
|
||||
next_setpoint_index++;
|
||||
}
|
||||
|
||||
/* populate last and next */
|
||||
|
||||
// triplet.previous_valid = false;
|
||||
// triplet.next_valid = false;
|
||||
|
||||
// if (last_setpoint_valid) {
|
||||
// triplet.previous_valid = true;
|
||||
// struct vehicle_global_position_setpoint_s sp;
|
||||
// sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
|
||||
// sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
|
||||
// sp.altitude = wpm->waypoints[last_setpoint_index].z;
|
||||
// sp.altitude_is_relative = false;
|
||||
// sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
|
||||
// set_special_fields(wpm->waypoints[last_setpoint_index].param1,
|
||||
// wpm->waypoints[last_setpoint_index].param2,
|
||||
// wpm->waypoints[last_setpoint_index].param3,
|
||||
// wpm->waypoints[last_setpoint_index].param4,
|
||||
// wpm->waypoints[last_setpoint_index].command, &sp);
|
||||
// memcpy(&(triplet.previous), &sp, sizeof(sp));
|
||||
// }
|
||||
|
||||
// if (next_setpoint_valid) {
|
||||
// triplet.next_valid = true;
|
||||
// struct vehicle_global_position_setpoint_s sp;
|
||||
// sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
|
||||
// sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
|
||||
// sp.altitude = wpm->waypoints[next_setpoint_index].z;
|
||||
// sp.altitude_is_relative = false;
|
||||
// sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
|
||||
// set_special_fields(wpm->waypoints[next_setpoint_index].param1,
|
||||
// wpm->waypoints[next_setpoint_index].param2,
|
||||
// wpm->waypoints[next_setpoint_index].param3,
|
||||
// wpm->waypoints[next_setpoint_index].param4,
|
||||
// wpm->waypoints[next_setpoint_index].command, &sp);
|
||||
// memcpy(&(triplet.next), &sp, sizeof(sp));
|
||||
// }
|
||||
|
||||
/* Initialize triplet publication if necessary */
|
||||
// if (global_position_set_triplet_pub < 0) {
|
||||
// global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
|
||||
|
||||
// } else {
|
||||
// orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
|
||||
// }
|
||||
|
||||
sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
|
||||
|
||||
} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
|
||||
/* global, relative alt (in relation to HOME) waypoint */
|
||||
struct vehicle_global_position_setpoint_s sp;
|
||||
sp.lat = param5_lat_x * 1e7f;
|
||||
sp.lon = param6_lon_y * 1e7f;
|
||||
sp.altitude = param7_alt_z;
|
||||
sp.altitude_is_relative = true;
|
||||
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
|
||||
set_special_fields(param1, param2, param3, param4, command, &sp);
|
||||
|
||||
/* Initialize publication if necessary */
|
||||
if (global_position_setpoint_pub < 0) {
|
||||
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
|
||||
}
|
||||
|
||||
|
||||
|
||||
sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
|
||||
|
||||
} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
|
||||
/* local, absolute waypoint */
|
||||
struct vehicle_local_position_setpoint_s sp;
|
||||
sp.x = param5_lat_x;
|
||||
sp.y = param6_lon_y;
|
||||
sp.z = param7_alt_z;
|
||||
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
|
||||
|
||||
/* Initialize publication if necessary */
|
||||
if (local_position_setpoint_pub < 0) {
|
||||
local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
|
||||
}
|
||||
|
||||
sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
|
||||
} else {
|
||||
warnx("non-navigation WP, ignoring");
|
||||
mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring.");
|
||||
return;
|
||||
}
|
||||
|
||||
/* only set this for known waypoint types (non-navigation types would have returned earlier) */
|
||||
last_waypoint_index = index;
|
||||
|
||||
mavlink_missionlib_send_gcs_string(buf);
|
||||
printf("%s\n", buf);
|
||||
//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -1,52 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file missionlib.h
|
||||
* MAVLink mission helper library
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "mavlink_bridge_header.h"
|
||||
|
||||
//extern void mavlink_wpm_send_message(mavlink_message_t *msg);
|
||||
//extern void mavlink_wpm_send_gcs_string(const char *string);
|
||||
//extern uint64_t mavlink_wpm_get_system_timestamp(void);
|
||||
extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
|
||||
extern int mavlink_missionlib_send_gcs_string(const char *string);
|
||||
extern uint64_t mavlink_missionlib_get_system_timestamp(void);
|
||||
extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
|
||||
float param2, float param3, float param4, float param5_lat_x,
|
||||
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
|
||||
|
|
@ -37,7 +37,6 @@
|
|||
|
||||
MODULE_COMMAND = mavlink
|
||||
SRCS += mavlink.c \
|
||||
missionlib.c \
|
||||
mavlink_parameters.c \
|
||||
mavlink_receiver.cpp \
|
||||
orb_listener.c \
|
||||
|
|
|
|||
|
|
@ -60,7 +60,6 @@
|
|||
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "missionlib.h"
|
||||
#include "mavlink_hil.h"
|
||||
#include "util.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -45,6 +45,7 @@
|
|||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -46,19 +46,10 @@
|
|||
or in the same folder as this source file */
|
||||
|
||||
#include <v1.0/mavlink_types.h>
|
||||
|
||||
// #ifndef MAVLINK_SEND_UART_BYTES
|
||||
// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
|
||||
// #endif
|
||||
//extern mavlink_system_t mavlink_system;
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include <stdbool.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
|
||||
// FIXME XXX - TO BE MOVED TO XML
|
||||
enum MAVLINK_WPM_STATES {
|
||||
MAVLINK_WPM_STATE_IDLE = 0,
|
||||
MAVLINK_WPM_STATE_SENDLIST,
|
||||
|
|
@ -79,44 +70,24 @@ enum MAVLINK_WPM_CODES {
|
|||
};
|
||||
|
||||
|
||||
/* WAYPOINT MANAGER - MISSION LIB */
|
||||
|
||||
#define MAVLINK_WPM_MAX_WP_COUNT 15
|
||||
// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
|
||||
#ifndef MAVLINK_WPM_TEXT_FEEDBACK
|
||||
#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text
|
||||
#endif
|
||||
#define MAVLINK_WPM_MAX_WP_COUNT 255
|
||||
#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds
|
||||
#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
|
||||
#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
|
||||
|
||||
|
||||
struct mavlink_wpm_storage {
|
||||
mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints
|
||||
// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
|
||||
// mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
|
||||
// #endif
|
||||
uint16_t size;
|
||||
uint16_t max_size;
|
||||
uint16_t rcv_size;
|
||||
enum MAVLINK_WPM_STATES current_state;
|
||||
int16_t current_wp_id; ///< Waypoint in current transmission
|
||||
// int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards
|
||||
uint16_t current_count;
|
||||
uint8_t current_partner_sysid;
|
||||
uint8_t current_partner_compid;
|
||||
uint64_t timestamp_lastaction;
|
||||
// uint64_t timestamp_last_send_setpoint;
|
||||
// uint64_t timestamp_firstinside_orbit;
|
||||
// uint64_t timestamp_lastoutside_orbit;
|
||||
uint64_t timestamp_last_send_setpoint;
|
||||
uint32_t timeout;
|
||||
int current_dataman_id;
|
||||
// uint32_t delay_setpoint;
|
||||
// float accept_range_yaw;
|
||||
// float accept_range_distance;
|
||||
// bool yaw_reached;
|
||||
// bool pos_reached;
|
||||
// bool idle;
|
||||
};
|
||||
|
||||
typedef struct mavlink_wpm_storage mavlink_wpm_storage;
|
||||
|
|
@ -126,13 +97,16 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio
|
|||
|
||||
|
||||
void mavlink_wpm_init(mavlink_wpm_storage *state);
|
||||
int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
|
||||
struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
|
||||
void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos ,
|
||||
struct vehicle_local_position_s *local_pos);
|
||||
void mavlink_waypoint_eventloop(uint64_t now);
|
||||
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
|
||||
|
||||
extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
|
||||
float param2, float param3, float param4, float param5_lat_x,
|
||||
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
|
||||
|
||||
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
|
||||
|
||||
void mavlink_missionlib_send_message(mavlink_message_t *msg);
|
||||
int mavlink_missionlib_send_gcs_string(const char *string);
|
||||
|
||||
#endif /* WAYPOINTS_H_ */
|
||||
|
|
|
|||
|
|
@ -977,9 +977,8 @@ Navigator::start_mission()
|
|||
mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
|
||||
}
|
||||
} else {
|
||||
/* since a mission is not started without WPs available, this is not supposed to happen */
|
||||
/* since a mission is started without knowledge if there are more mission items available, this can fail */
|
||||
_mission_item_triplet.current_valid = false;
|
||||
warnx("ERROR: current WP can't be set");
|
||||
}
|
||||
|
||||
ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
|
||||
|
|
@ -1308,11 +1307,9 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const
|
|||
fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON &&
|
||||
fabsf(a.radius - b.radius) < FLT_EPSILON &&
|
||||
fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
|
||||
fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON &&
|
||||
fabsf(a.index - b.index) < FLT_EPSILON) {
|
||||
fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) {
|
||||
return true;
|
||||
} else {
|
||||
warnx("a.index %d, b.index %d", a.index, b.index);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -584,6 +584,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
/* this option is normally set last */
|
||||
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||
uint8_t count = 0;
|
||||
bool disabled = false;
|
||||
|
||||
/* assert min..center..max ordering */
|
||||
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
|
||||
|
|
@ -602,7 +603,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
|
||||
count++;
|
||||
}
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
|
||||
disabled = true;
|
||||
} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
|
||||
count++;
|
||||
}
|
||||
|
||||
|
|
@ -610,7 +614,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
if (count) {
|
||||
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
} else {
|
||||
} else if (!disabled) {
|
||||
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -104,49 +104,49 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
|
|||
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f);
|
||||
PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC9_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC9_TRIM, 1500);
|
||||
|
|
|
|||
|
|
@ -91,7 +91,6 @@ struct mission_item_s
|
|||
float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
|
||||
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
|
||||
bool autocontinue; /**< true if next waypoint should follow after this one */
|
||||
int index; /**< index matching the mavlink waypoint */
|
||||
enum ORIGIN origin; /**< where the waypoint has been generated */
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue