forked from Archive/PX4-Autopilot
Compare commits
20 Commits
main
...
release/1.
Author | SHA1 | Date |
---|---|---|
Roman | f13bbacd52 | |
Daniel Agar | 82aa24adfc | |
Daniel Agar | a7969de738 | |
Daniel Agar | 33e86d25b9 | |
Daniel Agar | 805dfc4312 | |
Daniel Agar | ab2d595224 | |
Philipp Oettershagen | ab044e274d | |
Daniel Agar | b0f766d90e | |
Daniel Agar | 2bb9d7e91f | |
Beat Küng | 5f8c08db79 | |
DanielePettenuzzo | a936dc291e | |
DanielePettenuzzo | 42dc2bd890 | |
DanielePettenuzzo | 9d878c719d | |
DanielePettenuzzo | 649d4be185 | |
DanielePettenuzzo | bec74085f1 | |
DanielePettenuzzo | 350df41e42 | |
DanielePettenuzzo | 95295b30e8 | |
DanielePettenuzzo | dd044ed4be | |
DanielePettenuzzo | 9b2e32c976 | |
Roman | e1bca8d01a |
|
@ -343,11 +343,7 @@ if [ ${VEHICLE_TYPE} == fw -o ${VEHICLE_TYPE} == vtol ]
|
||||||
then
|
then
|
||||||
if param compare CBRK_AIRSPD_CHK 0
|
if param compare CBRK_AIRSPD_CHK 0
|
||||||
then
|
then
|
||||||
if sdp3x_airspeed start
|
sdp3x_airspeed start -a
|
||||||
then
|
|
||||||
else
|
|
||||||
sdp3x_airspeed start -b 2
|
|
||||||
fi
|
|
||||||
|
|
||||||
# Pixhawk 2.1 has a MS5611 on I2C which gets wrongly
|
# Pixhawk 2.1 has a MS5611 on I2C which gets wrongly
|
||||||
# detected as MS5525 because the chip manufacturer was so
|
# detected as MS5525 because the chip manufacturer was so
|
||||||
|
@ -357,20 +353,13 @@ then
|
||||||
then
|
then
|
||||||
ms5525_airspeed start -b 2
|
ms5525_airspeed start -b 2
|
||||||
else
|
else
|
||||||
ms5525_airspeed start
|
ms5525_airspeed start -a
|
||||||
fi
|
fi
|
||||||
|
|
||||||
if ms4525_airspeed start
|
ms4525_airspeed start -a
|
||||||
then
|
|
||||||
else
|
ets_airspeed start -a
|
||||||
ms4525_airspeed start -b 2
|
|
||||||
fi
|
|
||||||
|
|
||||||
if ets_airspeed start
|
|
||||||
then
|
|
||||||
else
|
|
||||||
ets_airspeed start -b 1
|
|
||||||
fi
|
|
||||||
fi
|
fi
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
|
|
@ -239,19 +239,58 @@ namespace ets_airspeed
|
||||||
|
|
||||||
ETSAirspeed *g_dev;
|
ETSAirspeed *g_dev;
|
||||||
|
|
||||||
int start(int i2c_bus);
|
int bus_options[] = {
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION
|
||||||
|
PX4_I2C_BUS_EXPANSION,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION1
|
||||||
|
PX4_I2C_BUS_EXPANSION1,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION2
|
||||||
|
PX4_I2C_BUS_EXPANSION2,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_ONBOARD
|
||||||
|
PX4_I2C_BUS_ONBOARD,
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
|
||||||
|
|
||||||
|
int start();
|
||||||
|
int start_bus(int i2c_bus);
|
||||||
int stop();
|
int stop();
|
||||||
int reset();
|
int reset();
|
||||||
int info();
|
int info();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Start the driver.
|
* Attempt to start driver on all available I2C busses.
|
||||||
|
*
|
||||||
|
* This function will return as soon as the first sensor
|
||||||
|
* is detected on one of the available busses or if no
|
||||||
|
* sensors are detected.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
int
|
||||||
|
start()
|
||||||
|
{
|
||||||
|
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||||
|
if (start_bus(bus_options[i]) == PX4_OK) {
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return PX4_ERROR;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Start the driver on a specific bus.
|
||||||
*
|
*
|
||||||
* This function only returns if the sensor is up and running
|
* This function only returns if the sensor is up and running
|
||||||
* or could not be detected successfully.
|
* or could not be detected successfully.
|
||||||
*/
|
*/
|
||||||
int
|
int
|
||||||
start(int i2c_bus)
|
start_bus(int i2c_bus)
|
||||||
{
|
{
|
||||||
int fd;
|
int fd;
|
||||||
|
|
||||||
|
@ -349,6 +388,7 @@ ets_airspeed_usage()
|
||||||
PX4_INFO("usage: ets_airspeed command [options]");
|
PX4_INFO("usage: ets_airspeed command [options]");
|
||||||
PX4_INFO("options:");
|
PX4_INFO("options:");
|
||||||
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
|
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
|
||||||
|
PX4_INFO("\t-a --all");
|
||||||
PX4_INFO("command:");
|
PX4_INFO("command:");
|
||||||
PX4_INFO("\tstart|stop|reset|info");
|
PX4_INFO("\tstart|stop|reset|info");
|
||||||
}
|
}
|
||||||
|
@ -361,13 +401,18 @@ ets_airspeed_main(int argc, char *argv[])
|
||||||
int myoptind = 1;
|
int myoptind = 1;
|
||||||
int ch;
|
int ch;
|
||||||
const char *myoptarg = nullptr;
|
const char *myoptarg = nullptr;
|
||||||
|
bool start_all = false;
|
||||||
|
|
||||||
while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
|
while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
|
||||||
switch (ch) {
|
switch (ch) {
|
||||||
case 'b':
|
case 'b':
|
||||||
i2c_bus = atoi(myoptarg);
|
i2c_bus = atoi(myoptarg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case 'a':
|
||||||
|
start_all = true;
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
ets_airspeed_usage();
|
ets_airspeed_usage();
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -383,7 +428,12 @@ ets_airspeed_main(int argc, char *argv[])
|
||||||
* Start/load the driver.
|
* Start/load the driver.
|
||||||
*/
|
*/
|
||||||
if (!strcmp(argv[myoptind], "start")) {
|
if (!strcmp(argv[myoptind], "start")) {
|
||||||
return ets_airspeed::start(i2c_bus);
|
if (start_all) {
|
||||||
|
return ets_airspeed::start();
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return ets_airspeed::start_bus(i2c_bus);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -81,6 +81,7 @@
|
||||||
#define MEAS_DRIVER_FILTER_FREQ 1.2f
|
#define MEAS_DRIVER_FILTER_FREQ 1.2f
|
||||||
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
|
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
|
||||||
|
|
||||||
|
|
||||||
class MEASAirspeed : public Airspeed
|
class MEASAirspeed : public Airspeed
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -373,18 +374,57 @@ namespace meas_airspeed
|
||||||
|
|
||||||
MEASAirspeed *g_dev = nullptr;
|
MEASAirspeed *g_dev = nullptr;
|
||||||
|
|
||||||
int start(int i2c_bus);
|
int bus_options[] = {
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION
|
||||||
|
PX4_I2C_BUS_EXPANSION,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION1
|
||||||
|
PX4_I2C_BUS_EXPANSION1,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION2
|
||||||
|
PX4_I2C_BUS_EXPANSION2,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_ONBOARD
|
||||||
|
PX4_I2C_BUS_ONBOARD,
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
|
||||||
|
|
||||||
|
int start();
|
||||||
|
int start_bus(int i2c_bus);
|
||||||
int stop();
|
int stop();
|
||||||
int reset();
|
int reset();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Start the driver.
|
* Attempt to start driver on all available I2C busses.
|
||||||
|
*
|
||||||
|
* This function will return as soon as the first sensor
|
||||||
|
* is detected on one of the available busses or if no
|
||||||
|
* sensors are detected.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
int
|
||||||
|
start()
|
||||||
|
{
|
||||||
|
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||||
|
if (start_bus(bus_options[i]) == PX4_OK) {
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return PX4_ERROR;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Start the driver on a specific bus.
|
||||||
*
|
*
|
||||||
* This function call only returns once the driver is up and running
|
* This function call only returns once the driver is up and running
|
||||||
* or failed to detect the sensor.
|
* or failed to detect the sensor.
|
||||||
*/
|
*/
|
||||||
int
|
int
|
||||||
start(int i2c_bus)
|
start_bus(int i2c_bus)
|
||||||
{
|
{
|
||||||
int fd;
|
int fd;
|
||||||
|
|
||||||
|
@ -483,6 +523,7 @@ meas_airspeed_usage()
|
||||||
PX4_INFO("usage: meas_airspeed command [options]");
|
PX4_INFO("usage: meas_airspeed command [options]");
|
||||||
PX4_INFO("options:");
|
PX4_INFO("options:");
|
||||||
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
|
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
|
||||||
|
PX4_INFO("\t-a --all");
|
||||||
PX4_INFO("command:");
|
PX4_INFO("command:");
|
||||||
PX4_INFO("\tstart|stop|reset");
|
PX4_INFO("\tstart|stop|reset");
|
||||||
}
|
}
|
||||||
|
@ -496,12 +537,18 @@ ms4525_airspeed_main(int argc, char *argv[])
|
||||||
int ch;
|
int ch;
|
||||||
const char *myoptarg = nullptr;
|
const char *myoptarg = nullptr;
|
||||||
|
|
||||||
while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
|
bool start_all = false;
|
||||||
|
|
||||||
|
while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
|
||||||
switch (ch) {
|
switch (ch) {
|
||||||
case 'b':
|
case 'b':
|
||||||
i2c_bus = atoi(myoptarg);
|
i2c_bus = atoi(myoptarg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case 'a':
|
||||||
|
start_all = true;
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
meas_airspeed_usage();
|
meas_airspeed_usage();
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -517,7 +564,13 @@ ms4525_airspeed_main(int argc, char *argv[])
|
||||||
* Start/load the driver.
|
* Start/load the driver.
|
||||||
*/
|
*/
|
||||||
if (!strcmp(argv[myoptind], "start")) {
|
if (!strcmp(argv[myoptind], "start")) {
|
||||||
return meas_airspeed::start(i2c_bus);
|
if (start_all) {
|
||||||
|
return meas_airspeed::start();
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return meas_airspeed::start_bus(i2c_bus);
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -43,15 +43,56 @@ namespace ms5525_airspeed
|
||||||
{
|
{
|
||||||
MS5525 *g_dev = nullptr;
|
MS5525 *g_dev = nullptr;
|
||||||
|
|
||||||
int start(uint8_t i2c_bus);
|
int bus_options[] = {
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION
|
||||||
|
PX4_I2C_BUS_EXPANSION,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION1
|
||||||
|
PX4_I2C_BUS_EXPANSION1,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION2
|
||||||
|
PX4_I2C_BUS_EXPANSION2,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_ONBOARD
|
||||||
|
PX4_I2C_BUS_ONBOARD,
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
|
||||||
|
|
||||||
|
int start();
|
||||||
|
int start_bus(uint8_t i2c_bus);
|
||||||
int stop();
|
int stop();
|
||||||
int reset();
|
int reset();
|
||||||
|
|
||||||
// Start the driver.
|
/**
|
||||||
// This function call only returns once the driver is up and running
|
* Attempt to start driver on all available I2C busses.
|
||||||
// or failed to detect the sensor.
|
*
|
||||||
|
* This function will return as soon as the first sensor
|
||||||
|
* is detected on one of the available busses or if no
|
||||||
|
* sensors are detected.
|
||||||
|
*
|
||||||
|
*/
|
||||||
int
|
int
|
||||||
start(uint8_t i2c_bus)
|
start()
|
||||||
|
{
|
||||||
|
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||||
|
if (start_bus(bus_options[i]) == PX4_OK) {
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Start the driver on a specific bus.
|
||||||
|
*
|
||||||
|
* This function call only returns once the driver is up and running
|
||||||
|
* or failed to detect the sensor.
|
||||||
|
*/
|
||||||
|
int
|
||||||
|
start_bus(uint8_t i2c_bus)
|
||||||
{
|
{
|
||||||
int fd = -1;
|
int fd = -1;
|
||||||
|
|
||||||
|
@ -141,11 +182,12 @@ int reset()
|
||||||
static void
|
static void
|
||||||
ms5525_airspeed_usage()
|
ms5525_airspeed_usage()
|
||||||
{
|
{
|
||||||
PX4_WARN("usage: ms5525_airspeed command [options]");
|
PX4_INFO("usage: ms5525_airspeed command [options]");
|
||||||
PX4_WARN("options:");
|
PX4_INFO("options:");
|
||||||
PX4_WARN("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
|
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
|
||||||
PX4_WARN("command:");
|
PX4_INFO("\t-a --all");
|
||||||
PX4_WARN("\tstart|stop|reset");
|
PX4_INFO("command:");
|
||||||
|
PX4_INFO("\tstart|stop|reset");
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
@ -156,13 +198,18 @@ ms5525_airspeed_main(int argc, char *argv[])
|
||||||
int myoptind = 1;
|
int myoptind = 1;
|
||||||
int ch;
|
int ch;
|
||||||
const char *myoptarg = nullptr;
|
const char *myoptarg = nullptr;
|
||||||
|
bool start_all = false;
|
||||||
|
|
||||||
while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
|
while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
|
||||||
switch (ch) {
|
switch (ch) {
|
||||||
case 'b':
|
case 'b':
|
||||||
i2c_bus = atoi(myoptarg);
|
i2c_bus = atoi(myoptarg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case 'a':
|
||||||
|
start_all = true;
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
ms5525_airspeed_usage();
|
ms5525_airspeed_usage();
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -178,7 +225,12 @@ ms5525_airspeed_main(int argc, char *argv[])
|
||||||
* Start/load the driver.
|
* Start/load the driver.
|
||||||
*/
|
*/
|
||||||
if (!strcmp(argv[myoptind], "start")) {
|
if (!strcmp(argv[myoptind], "start")) {
|
||||||
return ms5525_airspeed::start(i2c_bus);
|
if (start_all) {
|
||||||
|
return ms5525_airspeed::start();
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return ms5525_airspeed::start_bus(i2c_bus);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -42,15 +42,56 @@ namespace sdp3x_airspeed
|
||||||
{
|
{
|
||||||
SDP3X *g_dev = nullptr;
|
SDP3X *g_dev = nullptr;
|
||||||
|
|
||||||
int start(uint8_t i2c_bus);
|
int bus_options[] = {
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION
|
||||||
|
PX4_I2C_BUS_EXPANSION,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION1
|
||||||
|
PX4_I2C_BUS_EXPANSION1,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION2
|
||||||
|
PX4_I2C_BUS_EXPANSION2,
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_ONBOARD
|
||||||
|
PX4_I2C_BUS_ONBOARD,
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
|
||||||
|
|
||||||
|
int start();
|
||||||
|
int start_bus(uint8_t i2c_bus);
|
||||||
int stop();
|
int stop();
|
||||||
int reset();
|
int reset();
|
||||||
|
|
||||||
// Start the driver.
|
/**
|
||||||
// This function call only returns once the driver is up and running
|
* Attempt to start driver on all available I2C busses.
|
||||||
// or failed to detect the sensor.
|
*
|
||||||
|
* This function will return as soon as the first sensor
|
||||||
|
* is detected on one of the available busses or if no
|
||||||
|
* sensors are detected.
|
||||||
|
*
|
||||||
|
*/
|
||||||
int
|
int
|
||||||
start(uint8_t i2c_bus)
|
start()
|
||||||
|
{
|
||||||
|
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||||
|
if (start_bus(bus_options[i]) == PX4_OK) {
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Start the driver on a specific bus.
|
||||||
|
*
|
||||||
|
* This function call only returns once the driver is up and running
|
||||||
|
* or failed to detect the sensor.
|
||||||
|
*/
|
||||||
|
int
|
||||||
|
start_bus(uint8_t i2c_bus)
|
||||||
{
|
{
|
||||||
int fd = -1;
|
int fd = -1;
|
||||||
|
|
||||||
|
@ -153,11 +194,12 @@ int reset()
|
||||||
static void
|
static void
|
||||||
sdp3x_airspeed_usage()
|
sdp3x_airspeed_usage()
|
||||||
{
|
{
|
||||||
PX4_WARN("usage: sdp3x_airspeed command [options]");
|
PX4_INFO("usage: sdp3x_airspeed command [options]");
|
||||||
PX4_WARN("options:");
|
PX4_INFO("options:");
|
||||||
PX4_WARN("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
|
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
|
||||||
PX4_WARN("command:");
|
PX4_INFO("\t-a --all");
|
||||||
PX4_WARN("\tstart|stop|reset");
|
PX4_INFO("command:");
|
||||||
|
PX4_INFO("\tstart|stop|reset");
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
@ -168,13 +210,18 @@ sdp3x_airspeed_main(int argc, char *argv[])
|
||||||
int myoptind = 1;
|
int myoptind = 1;
|
||||||
int ch;
|
int ch;
|
||||||
const char *myoptarg = nullptr;
|
const char *myoptarg = nullptr;
|
||||||
|
bool start_all = false;
|
||||||
|
|
||||||
while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
|
while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
|
||||||
switch (ch) {
|
switch (ch) {
|
||||||
case 'b':
|
case 'b':
|
||||||
i2c_bus = atoi(myoptarg);
|
i2c_bus = atoi(myoptarg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case 'a':
|
||||||
|
start_all = true;
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
sdp3x_airspeed_usage();
|
sdp3x_airspeed_usage();
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -191,7 +238,12 @@ sdp3x_airspeed_main(int argc, char *argv[])
|
||||||
* Start/load the driver.
|
* Start/load the driver.
|
||||||
*/
|
*/
|
||||||
if (!strcmp(argv[myoptind], "start")) {
|
if (!strcmp(argv[myoptind], "start")) {
|
||||||
return sdp3x_airspeed::start(i2c_bus);
|
if (start_all) {
|
||||||
|
return sdp3x_airspeed::start();
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return sdp3x_airspeed::start_bus(i2c_bus);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -515,8 +515,6 @@ PX4FMU::init()
|
||||||
/* initialize PWM limit lib */
|
/* initialize PWM limit lib */
|
||||||
pwm_limit_init(&_pwm_limit);
|
pwm_limit_init(&_pwm_limit);
|
||||||
|
|
||||||
update_pwm_rev_mask();
|
|
||||||
|
|
||||||
#ifdef RC_SERIAL_PORT
|
#ifdef RC_SERIAL_PORT
|
||||||
|
|
||||||
# ifdef RF_RADIO_POWER_CONTROL
|
# ifdef RF_RADIO_POWER_CONTROL
|
||||||
|
@ -537,12 +535,6 @@ PX4FMU::init()
|
||||||
// Getting initial parameter values
|
// Getting initial parameter values
|
||||||
update_params();
|
update_params();
|
||||||
|
|
||||||
for (unsigned i = 0; i < _max_actuators; i++) {
|
|
||||||
char pname[16];
|
|
||||||
sprintf(pname, "PWM_AUX_TRIM%d", i + 1);
|
|
||||||
param_find(pname);
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -926,15 +918,28 @@ PX4FMU::update_pwm_rev_mask()
|
||||||
{
|
{
|
||||||
_reverse_pwm_mask = 0;
|
_reverse_pwm_mask = 0;
|
||||||
|
|
||||||
|
const char *pname_format;
|
||||||
|
|
||||||
|
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||||
|
pname_format = "PWM_MAIN_REV%d";
|
||||||
|
|
||||||
|
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
|
||||||
|
pname_format = "PWM_AUX_REV%d";
|
||||||
|
|
||||||
|
} else {
|
||||||
|
PX4_ERR("PWM REV only for MAIN and AUX");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; i < _max_actuators; i++) {
|
for (unsigned i = 0; i < _max_actuators; i++) {
|
||||||
char pname[16];
|
char pname[16];
|
||||||
int32_t ival;
|
|
||||||
|
|
||||||
/* fill the channel reverse mask from parameters */
|
/* fill the channel reverse mask from parameters */
|
||||||
sprintf(pname, "PWM_AUX_REV%d", i + 1);
|
sprintf(pname, pname_format, i + 1);
|
||||||
param_t param_h = param_find(pname);
|
param_t param_h = param_find(pname);
|
||||||
|
|
||||||
if (param_h != PARAM_INVALID) {
|
if (param_h != PARAM_INVALID) {
|
||||||
|
int32_t ival = 0;
|
||||||
param_get(param_h, &ival);
|
param_get(param_h, &ival);
|
||||||
_reverse_pwm_mask |= ((int16_t)(ival != 0)) << i;
|
_reverse_pwm_mask |= ((int16_t)(ival != 0)) << i;
|
||||||
}
|
}
|
||||||
|
@ -950,15 +955,28 @@ PX4FMU::update_pwm_trims()
|
||||||
|
|
||||||
int16_t values[_max_actuators] = {};
|
int16_t values[_max_actuators] = {};
|
||||||
|
|
||||||
|
const char *pname_format;
|
||||||
|
|
||||||
|
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||||
|
pname_format = "PWM_MAIN_TRIM%d";
|
||||||
|
|
||||||
|
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
|
||||||
|
pname_format = "PWM_AUX_TRIM%d";
|
||||||
|
|
||||||
|
} else {
|
||||||
|
PX4_ERR("PWM TRIM only for MAIN and AUX");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; i < _max_actuators; i++) {
|
for (unsigned i = 0; i < _max_actuators; i++) {
|
||||||
char pname[16];
|
char pname[16];
|
||||||
float pval;
|
|
||||||
|
|
||||||
/* fill the struct from parameters */
|
/* fill the struct from parameters */
|
||||||
sprintf(pname, "PWM_AUX_TRIM%d", i + 1);
|
sprintf(pname, pname_format, i + 1);
|
||||||
param_t param_h = param_find(pname);
|
param_t param_h = param_find(pname);
|
||||||
|
|
||||||
if (param_h != PARAM_INVALID) {
|
if (param_h != PARAM_INVALID) {
|
||||||
|
float pval = 0.0f;
|
||||||
param_get(param_h, &pval);
|
param_get(param_h, &pval);
|
||||||
values[i] = (int16_t)(10000 * pval);
|
values[i] = (int16_t)(10000 * pval);
|
||||||
PX4_DEBUG("%s: %d", pname, values[i]);
|
PX4_DEBUG("%s: %d", pname, values[i]);
|
||||||
|
|
|
@ -31,165 +31,6 @@
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of aux output channel 1
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_REV1, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of aux output channel 2
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_REV2, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of aux output channel 3
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_REV3, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of aux output channel 4
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_REV4, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of aux output channel 5
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_REV5, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of aux output channel 6
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_REV6, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for FMU PWM output channel 1
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM1, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for FMU PWM output channel 2
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM2, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for FMU PWM output channel 3
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM3, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for FMU PWM output channel 4
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM4, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for FMU PWM output channel 5
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM5, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for FMU PWM output channel 6
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM6, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Thrust to PWM model parameter
|
|
||||||
*
|
|
||||||
* Parameter used to model the relationship between static thrust and motor
|
|
||||||
* input PWM. Model is: thrust = (1-factor)*PWM + factor * PWM^2
|
|
||||||
*
|
|
||||||
* @min 0.0
|
|
||||||
* @max 1.0
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.0f);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Minimum motor rise time (slew rate limit).
|
|
||||||
*
|
|
||||||
* Minimum time allowed for the motor input signal to pass through
|
|
||||||
* a range of 1000 PWM units. A value x means that the motor signal
|
|
||||||
* can only go from 1000 to 2000 PWM in maximum x seconds.
|
|
||||||
*
|
|
||||||
* Zero means that slew rate limiting is disabled.
|
|
||||||
*
|
|
||||||
* @min 0.0
|
|
||||||
* @unit s/(1000*PWM)
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Motor Ordering
|
* Motor Ordering
|
||||||
*
|
*
|
||||||
|
|
|
@ -55,182 +55,6 @@
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(SYS_USE_IO, 1);
|
PARAM_DEFINE_INT32(SYS_USE_IO, 1);
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of main output channel 1
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_MAIN_REV1, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of main output channel 2
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_MAIN_REV2, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of main output channel 3
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_MAIN_REV3, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of main output channel 4
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_MAIN_REV4, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of main output channel 5
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_MAIN_REV5, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of main output channel 6
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_MAIN_REV6, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of main output channel 7
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Invert direction of main output channel 8
|
|
||||||
*
|
|
||||||
* Enable to invert the channel.
|
|
||||||
*
|
|
||||||
* @boolean
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for main output channel 1
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM1, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for main output channel 2
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM2, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for main output channel 3
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM3, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for main output channel 4
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM4, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for main output channel 5
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM5, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for main output channel 6
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM6, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for main output channel 7
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM7, 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Trim value for main output channel 8
|
|
||||||
*
|
|
||||||
* Set to normalized offset
|
|
||||||
*
|
|
||||||
* @min -0.2
|
|
||||||
* @max 0.2
|
|
||||||
* @decimal 2
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM8, 0);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* S.BUS out
|
* S.BUS out
|
||||||
*
|
*
|
||||||
|
|
|
@ -723,7 +723,7 @@ int TAP_ESC::task_spawn(int argc, char *argv[])
|
||||||
_task_id = px4_task_spawn_cmd("tap_esc",
|
_task_id = px4_task_spawn_cmd("tap_esc",
|
||||||
SCHED_DEFAULT,
|
SCHED_DEFAULT,
|
||||||
SCHED_PRIORITY_ACTUATOR_OUTPUTS,
|
SCHED_PRIORITY_ACTUATOR_OUTPUTS,
|
||||||
1100,
|
1180,
|
||||||
(px4_main_t)&run_trampoline,
|
(px4_main_t)&run_trampoline,
|
||||||
argv);
|
argv);
|
||||||
|
|
||||||
|
|
|
@ -65,10 +65,9 @@ set_source_files_properties(${px4_git_ver_header} PROPERTIES GENERATED TRUE)
|
||||||
add_custom_target(ver_gen ALL DEPENDS ${px4_git_ver_header})
|
add_custom_target(ver_gen ALL DEPENDS ${px4_git_ver_header})
|
||||||
|
|
||||||
# The URL for the elf file for crash logging
|
# The URL for the elf file for crash logging
|
||||||
|
set(BUILD_URI "localhost")
|
||||||
if (DEFINED ENV{BUILD_URI})
|
if (DEFINED ENV{BUILD_URI})
|
||||||
set(BUILD_URI $ENV{BUILD_URI})
|
set(BUILD_URI $ENV{BUILD_URI})
|
||||||
else()
|
|
||||||
set(BUILD_URI "localhost")
|
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
add_definitions(-DBUILD_URI=${BUILD_URI})
|
add_definitions(-DBUILD_URI=${BUILD_URI})
|
||||||
|
|
|
@ -516,9 +516,20 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
|
||||||
if (inst != self) {
|
if (inst != self) {
|
||||||
const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid);
|
const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid);
|
||||||
|
|
||||||
|
int target_system_id = 0;
|
||||||
|
int target_component_id = 233;
|
||||||
|
|
||||||
|
// might be nullptr if message is unknown
|
||||||
|
if (meta) {
|
||||||
// Extract target system and target component if set
|
// Extract target system and target component if set
|
||||||
unsigned target_system_id = (meta->target_system_ofs != 0) ? ((uint8_t *)msg)[meta->target_system_ofs] : 0;
|
if (meta->target_system_ofs != 0) {
|
||||||
unsigned target_component_id = (meta->target_component_ofs != 0) ? ((uint8_t *)msg)[meta->target_component_ofs] : 233;
|
target_system_id = ((uint8_t *)msg)[meta->target_system_ofs];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (meta->target_component_ofs != 0) {
|
||||||
|
target_component_id = ((uint8_t *)msg)[meta->target_component_ofs];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// Broadcast or addressing this system and not trying to talk
|
// Broadcast or addressing this system and not trying to talk
|
||||||
// to the autopilot component -> pass on to other components
|
// to the autopilot component -> pass on to other components
|
||||||
|
|
|
@ -1023,9 +1023,6 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
MavlinkOrbSubscription *_att_sub;
|
|
||||||
uint64_t _att_time;
|
|
||||||
|
|
||||||
MavlinkOrbSubscription *_pos_sub;
|
MavlinkOrbSubscription *_pos_sub;
|
||||||
uint64_t _pos_time;
|
uint64_t _pos_time;
|
||||||
|
|
||||||
|
@ -1046,8 +1043,6 @@ private:
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink),
|
explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||||
_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
|
|
||||||
_att_time(0),
|
|
||||||
_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
|
_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
|
||||||
_pos_time(0),
|
_pos_time(0),
|
||||||
_armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))),
|
_armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))),
|
||||||
|
@ -1061,24 +1056,20 @@ protected:
|
||||||
|
|
||||||
bool send(const hrt_abstime t)
|
bool send(const hrt_abstime t)
|
||||||
{
|
{
|
||||||
vehicle_attitude_s att = {};
|
|
||||||
vehicle_local_position_s pos = {};
|
vehicle_local_position_s pos = {};
|
||||||
actuator_armed_s armed = {};
|
actuator_armed_s armed = {};
|
||||||
airspeed_s airspeed = {};
|
airspeed_s airspeed = {};
|
||||||
|
|
||||||
|
|
||||||
bool updated = false;
|
bool updated = false;
|
||||||
updated |= _att_sub->update(&_att_time, &att);
|
|
||||||
updated |= _pos_sub->update(&_pos_time, &pos);
|
updated |= _pos_sub->update(&_pos_time, &pos);
|
||||||
updated |= _armed_sub->update(&_armed_time, &armed);
|
updated |= _armed_sub->update(&_armed_time, &armed);
|
||||||
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
|
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
|
||||||
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
mavlink_vfr_hud_t msg = {};
|
mavlink_vfr_hud_t msg = {};
|
||||||
matrix::Eulerf euler = matrix::Quatf(att.q);
|
|
||||||
msg.airspeed = airspeed.indicated_airspeed_m_s;
|
msg.airspeed = airspeed.indicated_airspeed_m_s;
|
||||||
msg.groundspeed = sqrtf(pos.vx * pos.vx + pos.vy * pos.vy);
|
msg.groundspeed = sqrtf(pos.vx * pos.vx + pos.vy * pos.vy);
|
||||||
msg.heading = math::degrees(euler.psi());
|
msg.heading = math::degrees(wrap_2pi(pos.yaw));
|
||||||
|
|
||||||
if (armed.armed) {
|
if (armed.armed) {
|
||||||
actuator_controls_s act0 = {};
|
actuator_controls_s act0 = {};
|
||||||
|
@ -1791,7 +1782,7 @@ protected:
|
||||||
msg.vy = lpos.vy * 100.0f;
|
msg.vy = lpos.vy * 100.0f;
|
||||||
msg.vz = lpos.vz * 100.0f;
|
msg.vz = lpos.vz * 100.0f;
|
||||||
|
|
||||||
msg.hdg = math::degrees(lpos.yaw) * 100.0f;
|
msg.hdg = math::degrees(wrap_2pi(lpos.yaw)) * 100.0f;
|
||||||
|
|
||||||
mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
|
mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
|
||||||
|
|
||||||
|
@ -3958,9 +3949,9 @@ protected:
|
||||||
|
|
||||||
mavlink_mount_orientation_t msg = {};
|
mavlink_mount_orientation_t msg = {};
|
||||||
|
|
||||||
msg.roll = 180.0f / M_PI_F * mount_orientation.attitude_euler_angle[0];
|
msg.roll = math::degrees(mount_orientation.attitude_euler_angle[0]);
|
||||||
msg.pitch = 180.0f / M_PI_F * mount_orientation.attitude_euler_angle[1];
|
msg.pitch = math::degrees(mount_orientation.attitude_euler_angle[1]);
|
||||||
msg.yaw = 180.0f / M_PI_F * mount_orientation.attitude_euler_angle[2];
|
msg.yaw = math::degrees(mount_orientation.attitude_euler_angle[2]);
|
||||||
|
|
||||||
mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg);
|
mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg);
|
||||||
|
|
||||||
|
|
|
@ -60,6 +60,15 @@ MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_sys
|
||||||
|
|
||||||
if (_ulog_stream_sub < 0) {
|
if (_ulog_stream_sub < 0) {
|
||||||
PX4_ERR("orb_subscribe failed (%i)", errno);
|
PX4_ERR("orb_subscribe failed (%i)", errno);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// make sure we won't read any old messages
|
||||||
|
struct ulog_stream_s stream_msg;
|
||||||
|
bool update;
|
||||||
|
|
||||||
|
while (orb_check(_ulog_stream_sub, &update) == 0 && update) {
|
||||||
|
orb_copy(ORB_ID(ulog_stream), _ulog_stream_sub, &stream_msg);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
_waiting_for_initial_ack = true;
|
_waiting_for_initial_ack = true;
|
||||||
|
@ -264,7 +273,7 @@ void MavlinkULog::publish_ack(uint16_t sequence)
|
||||||
ack.sequence = sequence;
|
ack.sequence = sequence;
|
||||||
|
|
||||||
if (_ulog_stream_ack_pub == nullptr) {
|
if (_ulog_stream_ack_pub == nullptr) {
|
||||||
_ulog_stream_ack_pub = orb_advertise_queue(ORB_ID(ulog_stream_ack), &ack, 3);
|
_ulog_stream_ack_pub = orb_advertise(ORB_ID(ulog_stream_ack), &ack);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(ulog_stream_ack), _ulog_stream_ack_pub, &ack);
|
orb_publish(ORB_ID(ulog_stream_ack), _ulog_stream_ack_pub, &ack);
|
||||||
|
|
|
@ -613,6 +613,14 @@ MulticopterAttitudeControl::run()
|
||||||
}
|
}
|
||||||
|
|
||||||
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
|
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
|
||||||
|
|
||||||
|
// sensor correction topic is not being published regularly and we might have missed the first update.
|
||||||
|
// so copy it once initially so that we have the latest data. In future this will not be needed anymore as the
|
||||||
|
// behavior of the orb_check function will change
|
||||||
|
if (_sensor_correction_sub > 0) {
|
||||||
|
orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &_sensor_correction);
|
||||||
|
}
|
||||||
|
|
||||||
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
|
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
|
||||||
|
|
||||||
/* wakeup source: gyro data from sensor selected by the sensor app */
|
/* wakeup source: gyro data from sensor selected by the sensor app */
|
||||||
|
|
|
@ -1297,8 +1297,15 @@ Mission::altitude_sp_foh_update()
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Calculate acceptance radius, i.e. the radius within which we do not perform a first order hold anymore
|
||||||
|
float acc_rad = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
|
||||||
|
|
||||||
|
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
|
||||||
|
acc_rad = _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f);
|
||||||
|
}
|
||||||
|
|
||||||
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
|
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
|
||||||
if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < FLT_EPSILON) {
|
if (_distance_current_previous - acc_rad < FLT_EPSILON) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1327,7 +1334,7 @@ Mission::altitude_sp_foh_update()
|
||||||
|
|
||||||
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
|
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
|
||||||
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
|
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
|
||||||
if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
|
if (_min_current_sp_distance_xy < acc_rad) {
|
||||||
pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
|
pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -1337,8 +1344,7 @@ Mission::altitude_sp_foh_update()
|
||||||
* radius around the current waypoint
|
* radius around the current waypoint
|
||||||
**/
|
**/
|
||||||
float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt);
|
float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt);
|
||||||
float grad = -delta_alt / (_distance_current_previous - _navigator->get_acceptance_radius(
|
float grad = -delta_alt / (_distance_current_previous - acc_rad);
|
||||||
_mission_item.acceptance_radius));
|
|
||||||
float a = pos_sp_triplet->previous.alt - grad * _distance_current_previous;
|
float a = pos_sp_triplet->previous.alt - grad * _distance_current_previous;
|
||||||
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
|
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
|
||||||
}
|
}
|
||||||
|
|
|
@ -0,0 +1,67 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2018 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 motor_params.c
|
||||||
|
*
|
||||||
|
* Parameters for motors.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Minimum motor rise time (slew rate limit).
|
||||||
|
*
|
||||||
|
* Minimum time allowed for the motor input signal to pass through
|
||||||
|
* a range of 1000 PWM units. A value x means that the motor signal
|
||||||
|
* can only go from 1000 to 2000 PWM in maximum x seconds.
|
||||||
|
*
|
||||||
|
* Zero means that slew rate limiting is disabled.
|
||||||
|
*
|
||||||
|
* @min 0.0
|
||||||
|
* @unit s/(1000*PWM)
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Thrust to PWM model parameter
|
||||||
|
*
|
||||||
|
* Parameter used to model the relationship between static thrust and motor
|
||||||
|
* input PWM. Model is: thrust = (1-factor)*PWM + factor * PWM^2
|
||||||
|
*
|
||||||
|
* @min 0.0
|
||||||
|
* @max 1.0
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.0f);
|
|
@ -0,0 +1,406 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2012-2016 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 pwm_params_aux.c
|
||||||
|
*
|
||||||
|
* Parameters defined for PWM output.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the PWM output frequency for the auxiliary outputs
|
||||||
|
*
|
||||||
|
* Set to 400 for industry default or 1000 for high frequency ESCs.
|
||||||
|
*
|
||||||
|
* Set to 0 for Oneshot125.
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min -1
|
||||||
|
* @max 2000
|
||||||
|
* @unit Hz
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_RATE, 50);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the minimum PWM for the auxiliary outputs
|
||||||
|
*
|
||||||
|
* Set to 1000 for industry default or 900 to increase servo travel.
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min 800
|
||||||
|
* @max 1400
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the maximum PWM for the auxiliary outputs
|
||||||
|
*
|
||||||
|
* Set to 2000 for industry default or 2100 to increase servo travel.
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min 1600
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the disarmed PWM for auxiliary outputs
|
||||||
|
*
|
||||||
|
* This is the PWM pulse the autopilot is outputting if not armed.
|
||||||
|
* The main use of this parameter is to silence ESCs when they are disarmed.
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min 0
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1500);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the disarmed PWM for the auxiliary 1 output
|
||||||
|
*
|
||||||
|
* This is the PWM pulse the autopilot is outputting if not armed.
|
||||||
|
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min -1
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_DIS1, -1);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the disarmed PWM for the auxiliary 2 output
|
||||||
|
*
|
||||||
|
* This is the PWM pulse the autopilot is outputting if not armed.
|
||||||
|
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min -1
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_DIS2, -1);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the disarmed PWM for the auxiliary 3 output
|
||||||
|
*
|
||||||
|
* This is the PWM pulse the autopilot is outputting if not armed.
|
||||||
|
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min -1
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_DIS3, -1);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the disarmed PWM for the auxiliary 4 output
|
||||||
|
*
|
||||||
|
* This is the PWM pulse the autopilot is outputting if not armed.
|
||||||
|
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min -1
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_DIS4, -1);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the disarmed PWM for the auxiliary 5 output
|
||||||
|
*
|
||||||
|
* This is the PWM pulse the autopilot is outputting if not armed.
|
||||||
|
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min -1
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_DIS5, -1);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the disarmed PWM for the auxiliary 6 output
|
||||||
|
*
|
||||||
|
* This is the PWM pulse the autopilot is outputting if not armed.
|
||||||
|
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min -1
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_DIS6, -1);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the disarmed PWM for the auxiliary 7 output
|
||||||
|
*
|
||||||
|
* This is the PWM pulse the autopilot is outputting if not armed.
|
||||||
|
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min -1
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_DIS7, -1);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the disarmed PWM for the auxiliary 8 output
|
||||||
|
*
|
||||||
|
* This is the PWM pulse the autopilot is outputting if not armed.
|
||||||
|
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
||||||
|
*
|
||||||
|
* @reboot_required true
|
||||||
|
*
|
||||||
|
* @min -1
|
||||||
|
* @max 2200
|
||||||
|
* @unit us
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_DIS8, -1);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Invert direction of auxiliary output channel 1
|
||||||
|
*
|
||||||
|
* Enable to invert the channel.
|
||||||
|
*
|
||||||
|
* @boolean
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_REV1, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Invert direction of auxiliary output channel 2
|
||||||
|
*
|
||||||
|
* Enable to invert the channel.
|
||||||
|
*
|
||||||
|
* @boolean
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_REV2, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Invert direction of auxiliary output channel 3
|
||||||
|
*
|
||||||
|
* Enable to invert the channel.
|
||||||
|
*
|
||||||
|
* @boolean
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_REV3, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Invert direction of auxiliary output channel 4
|
||||||
|
*
|
||||||
|
* Enable to invert the channel.
|
||||||
|
*
|
||||||
|
* @boolean
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_REV4, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Invert direction of auxiliary output channel 5
|
||||||
|
*
|
||||||
|
* Enable to invert the channel.
|
||||||
|
*
|
||||||
|
* @boolean
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_REV5, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Invert direction of auxiliary output channel 6
|
||||||
|
*
|
||||||
|
* Enable to invert the channel.
|
||||||
|
*
|
||||||
|
* @boolean
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_REV6, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Invert direction of auxiliary output channel 7
|
||||||
|
*
|
||||||
|
* Enable to invert the channel.
|
||||||
|
*
|
||||||
|
* @boolean
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_REV7, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Invert direction of auxiliary output channel 8
|
||||||
|
*
|
||||||
|
* Enable to invert the channel.
|
||||||
|
*
|
||||||
|
* @boolean
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(PWM_AUX_REV8, 0);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for auxiliary output channel 1
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM1, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for auxiliary output channel 2
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM2, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for auxiliary output channel 3
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM3, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for auxiliary output channel 4
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM4, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for auxiliary output channel 5
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM5, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for auxiliary output channel 6
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM6, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for auxiliary output channel 7
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM7, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for auxiliary output channel 8
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM8, 0);
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -32,7 +32,7 @@
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file pwm_params.c
|
* @file pwm_params_main.c
|
||||||
*
|
*
|
||||||
* Parameters defined for PWM output.
|
* Parameters defined for PWM output.
|
||||||
*
|
*
|
||||||
|
@ -97,6 +97,9 @@ PARAM_DEFINE_INT32(PWM_MAX, 2000);
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_DISARMED, 900);
|
PARAM_DEFINE_INT32(PWM_DISARMED, 900);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the disarmed PWM for the main 1 output
|
* Set the disarmed PWM for the main 1 output
|
||||||
*
|
*
|
||||||
|
@ -217,135 +220,187 @@ PARAM_DEFINE_INT32(PWM_MAIN_DIS7, -1);
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_MAIN_DIS8, -1);
|
PARAM_DEFINE_INT32(PWM_MAIN_DIS8, -1);
|
||||||
|
|
||||||
/**
|
|
||||||
* Set the disarmed PWM for the AUX 1 output
|
|
||||||
*
|
|
||||||
* This is the PWM pulse the autopilot is outputting if not armed.
|
|
||||||
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
|
||||||
*
|
|
||||||
* @reboot_required true
|
|
||||||
*
|
|
||||||
* @min -1
|
|
||||||
* @max 2200
|
|
||||||
* @unit us
|
|
||||||
* @group PWM Outputs
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_DIS1, -1);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the disarmed PWM for the AUX 2 output
|
* Invert direction of main output channel 1
|
||||||
*
|
*
|
||||||
* This is the PWM pulse the autopilot is outputting if not armed.
|
* Enable to invert the channel.
|
||||||
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
|
||||||
*
|
*
|
||||||
* @reboot_required true
|
* @boolean
|
||||||
*
|
|
||||||
* @min -1
|
|
||||||
* @max 2200
|
|
||||||
* @unit us
|
|
||||||
* @group PWM Outputs
|
* @group PWM Outputs
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_DIS2, -1);
|
PARAM_DEFINE_INT32(PWM_MAIN_REV1, 0);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the disarmed PWM for the AUX 3 output
|
* Invert direction of main output channel 2
|
||||||
*
|
*
|
||||||
* This is the PWM pulse the autopilot is outputting if not armed.
|
* Enable to invert the channel.
|
||||||
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
|
||||||
*
|
*
|
||||||
* @reboot_required true
|
* @boolean
|
||||||
*
|
|
||||||
* @min -1
|
|
||||||
* @max 2200
|
|
||||||
* @unit us
|
|
||||||
* @group PWM Outputs
|
* @group PWM Outputs
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_DIS3, -1);
|
PARAM_DEFINE_INT32(PWM_MAIN_REV2, 0);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the disarmed PWM for the AUX 4 output
|
* Invert direction of main output channel 3
|
||||||
*
|
*
|
||||||
* This is the PWM pulse the autopilot is outputting if not armed.
|
* Enable to invert the channel.
|
||||||
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
|
||||||
*
|
*
|
||||||
* @reboot_required true
|
* @boolean
|
||||||
*
|
|
||||||
* @min -1
|
|
||||||
* @max 2200
|
|
||||||
* @unit us
|
|
||||||
* @group PWM Outputs
|
* @group PWM Outputs
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_DIS4, -1);
|
PARAM_DEFINE_INT32(PWM_MAIN_REV3, 0);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the disarmed PWM for the AUX 5 output
|
* Invert direction of main output channel 4
|
||||||
*
|
*
|
||||||
* This is the PWM pulse the autopilot is outputting if not armed.
|
* Enable to invert the channel.
|
||||||
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
|
||||||
*
|
*
|
||||||
* @reboot_required true
|
* @boolean
|
||||||
*
|
|
||||||
* @min -1
|
|
||||||
* @max 2200
|
|
||||||
* @unit us
|
|
||||||
* @group PWM Outputs
|
* @group PWM Outputs
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_DIS5, -1);
|
PARAM_DEFINE_INT32(PWM_MAIN_REV4, 0);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the disarmed PWM for the AUX 6 output
|
* Invert direction of main output channel 5
|
||||||
*
|
*
|
||||||
* This is the PWM pulse the autopilot is outputting if not armed.
|
* Enable to invert the channel.
|
||||||
* When set to -1 the value for PWM_AUX_DISARMED will be used
|
|
||||||
*
|
*
|
||||||
* @reboot_required true
|
* @boolean
|
||||||
*
|
|
||||||
* @min -1
|
|
||||||
* @max 2200
|
|
||||||
* @unit us
|
|
||||||
* @group PWM Outputs
|
* @group PWM Outputs
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_DIS6, -1);
|
PARAM_DEFINE_INT32(PWM_MAIN_REV5, 0);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the minimum PWM for the auxiliary outputs
|
* Invert direction of main output channel 6
|
||||||
*
|
*
|
||||||
* Set to 1000 for default or 900 to increase servo travel
|
* Enable to invert the channel.
|
||||||
*
|
*
|
||||||
* @reboot_required true
|
* @boolean
|
||||||
*
|
|
||||||
* @min 800
|
|
||||||
* @max 1400
|
|
||||||
* @unit us
|
|
||||||
* @group PWM Outputs
|
* @group PWM Outputs
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000);
|
PARAM_DEFINE_INT32(PWM_MAIN_REV6, 0);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the maximum PWM for the auxiliary outputs
|
* Invert direction of main output channel 7
|
||||||
*
|
*
|
||||||
* Set to 2000 for default or 2100 to increase servo travel
|
* Enable to invert the channel.
|
||||||
*
|
*
|
||||||
* @reboot_required true
|
* @boolean
|
||||||
*
|
|
||||||
* @min 1600
|
|
||||||
* @max 2200
|
|
||||||
* @unit us
|
|
||||||
* @group PWM Outputs
|
* @group PWM Outputs
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000);
|
PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the disarmed PWM for auxiliary outputs
|
* Invert direction of main output channel 8
|
||||||
*
|
*
|
||||||
* This is the PWM pulse the autopilot is outputting if not armed.
|
* Enable to invert the channel.
|
||||||
* The main use of this parameter is to silence ESCs when they are disarmed.
|
|
||||||
*
|
*
|
||||||
* @reboot_required true
|
* @boolean
|
||||||
*
|
|
||||||
* @min 0
|
|
||||||
* @max 2200
|
|
||||||
* @unit us
|
|
||||||
* @group PWM Outputs
|
* @group PWM Outputs
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1500);
|
PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for main output channel 1
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM1, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for main output channel 2
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM2, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for main output channel 3
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM3, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for main output channel 4
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM4, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for main output channel 5
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM5, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for main output channel 6
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM6, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for main output channel 7
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM7, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Trim value for main output channel 8
|
||||||
|
*
|
||||||
|
* Set to normalized offset
|
||||||
|
*
|
||||||
|
* @min -0.2
|
||||||
|
* @max 0.2
|
||||||
|
* @decimal 2
|
||||||
|
* @group PWM Outputs
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM8, 0);
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue