aboutsummaryrefslogtreecommitdiff
path: root/apps/systemlib
diff options
context:
space:
mode:
Diffstat (limited to 'apps/systemlib')
-rw-r--r--apps/systemlib/bson/tinybson.c24
-rw-r--r--apps/systemlib/bson/tinybson.h27
-rw-r--r--apps/systemlib/conversions.c14
-rw-r--r--apps/systemlib/cpuload.c88
-rw-r--r--apps/systemlib/err.c16
-rw-r--r--apps/systemlib/err.h38
-rw-r--r--apps/systemlib/geo/geo.c120
-rw-r--r--apps/systemlib/geo/geo.h14
-rw-r--r--apps/systemlib/mixer/mixer.cpp2
-rw-r--r--apps/systemlib/mixer/mixer_group.cpp20
-rw-r--r--apps/systemlib/mixer/mixer_multirotor.cpp6
-rw-r--r--apps/systemlib/param/param.c8
-rw-r--r--apps/systemlib/pid/pid.c28
-rw-r--r--apps/systemlib/ppm_decode.c24
-rw-r--r--apps/systemlib/ppm_decode.h2
-rw-r--r--apps/systemlib/systemlib.c12
16 files changed, 254 insertions, 189 deletions
diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c
index 10b736fd6..75578d2ec 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/apps/systemlib/bson/tinybson.c
@@ -56,7 +56,7 @@
static int
read_int8(bson_decoder_t decoder, int8_t *b)
{
- return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1;
+ return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1;
}
static int
@@ -119,12 +119,14 @@ bson_decoder_next(bson_decoder_t decoder)
while (decoder->pending > 0) {
if (read_int8(decoder, &tbyte))
CODER_KILL(decoder, "read error discarding pending bytes");
+
decoder->pending--;
}
/* get the type byte */
if (read_int8(decoder, &tbyte))
CODER_KILL(decoder, "read error on type byte");
+
decoder->node.type = tbyte;
decoder->pending = 0;
@@ -135,13 +137,17 @@ bson_decoder_next(bson_decoder_t decoder)
/* get the node name */
nlen = 0;
+
for (;;) {
if (nlen >= BSON_MAXNAME)
CODER_KILL(decoder, "node name overflow");
+
if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen]))
CODER_KILL(decoder, "read error on node name");
+
if (decoder->node.name[nlen] == '\0')
break;
+
nlen++;
}
@@ -151,20 +157,28 @@ bson_decoder_next(bson_decoder_t decoder)
case BSON_INT:
if (read_int32(decoder, &decoder->node.i))
CODER_KILL(decoder, "read error on BSON_INT");
+
break;
+
case BSON_DOUBLE:
if (read_double(decoder, &decoder->node.d))
CODER_KILL(decoder, "read error on BSON_DOUBLE");
+
break;
+
case BSON_STRING:
if (read_int32(decoder, &decoder->pending))
CODER_KILL(decoder, "read error on BSON_STRING length");
+
break;
+
case BSON_BINDATA:
if (read_int32(decoder, &decoder->pending))
CODER_KILL(decoder, "read error on BSON_BINDATA size");
+
if (read_int8(decoder, &tbyte))
CODER_KILL(decoder, "read error on BSON_BINDATA subtype");
+
decoder->node.subtype = tbyte;
break;
@@ -186,11 +200,12 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf)
CODER_CHECK(decoder);
/* if data already copied, return zero bytes */
- if (decoder->pending == 0)
+ if (decoder->pending == 0)
return 0;
/* copy bytes per the node size */
result = read(decoder->fd, buf, decoder->pending);
+
if (result != decoder->pending)
CODER_KILL(decoder, "read error on copy_data");
@@ -209,7 +224,7 @@ static int
write_int8(bson_encoder_t encoder, int8_t b)
{
debug("write_int8 %d", b);
- return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1;
+ return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1;
}
static int
@@ -233,6 +248,7 @@ write_name(bson_encoder_t encoder, const char *name)
if (len > BSON_MAXNAME)
return -1;
+
debug("write name '%s' len %d", name, len);
return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1;
}
@@ -300,6 +316,7 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char
write_int32(encoder, len) ||
write(encoder->fd, name, len + 1) != (int)(len + 1))
CODER_KILL(encoder, "write error on BSON_STRING");
+
return 0;
}
@@ -314,5 +331,6 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary
write_int8(encoder, subtype) ||
write(encoder->fd, data, size) != (int)(size))
CODER_KILL(encoder, "write error on BSON_BINDATA");
+
return 0;
}
diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h
index 1b9de5cd3..b6229dc50 100644
--- a/apps/systemlib/bson/tinybson.h
+++ b/apps/systemlib/bson/tinybson.h
@@ -31,14 +31,14 @@
*
****************************************************************************/
- /**
- * @file tinybson.h
- *
- * A simple subset SAX-style BSON parser and generator. See http://bsonspec.org
- *
- * Some types and defines taken from the standalone BSON parser/generator
- * in the Mongo C connector.
- */
+/**
+* @file tinybson.h
+*
+* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org
+*
+* Some types and defines taken from the standalone BSON parser/generator
+* in the Mongo C connector.
+*/
#ifndef _TINYBSON_H
#define _TINYBSON_H
@@ -77,8 +77,7 @@ typedef enum bson_binary_subtype {
/**
* Node structure passed to the callback.
*/
-typedef struct bson_node_s
-{
+typedef struct bson_node_s {
char name[BSON_MAXNAME];
bson_type_t type;
bson_binary_subtype_t subtype;
@@ -96,8 +95,7 @@ typedef struct bson_decoder_s *bson_decoder_t;
*/
typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node);
-struct bson_decoder_s
-{
+struct bson_decoder_s {
int fd;
bson_decoder_callback callback;
void *private;
@@ -143,8 +141,7 @@ __EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder);
/**
* Encoder state structure.
*/
-typedef struct bson_encoder_s
-{
+typedef struct bson_encoder_s {
int fd;
} *bson_encoder_t;
@@ -169,7 +166,7 @@ __EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, i
*/
__EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value);
-/**
+/**
* Append a string to the encoded stream.
*/
__EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string);
diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c
index 99ee41508..9105d83cb 100644
--- a/apps/systemlib/conversions.c
+++ b/apps/systemlib/conversions.c
@@ -45,13 +45,13 @@
int16_t
int16_t_from_bytes(uint8_t bytes[])
{
- union {
- uint8_t b[2];
- int16_t w;
- } u;
+ union {
+ uint8_t b[2];
+ int16_t w;
+ } u;
- u.b[1] = bytes[0];
- u.b[0] = bytes[1];
+ u.b[1] = bytes[0];
+ u.b[0] = bytes[1];
- return u.w;
+ return u.w;
}
diff --git a/apps/systemlib/cpuload.c b/apps/systemlib/cpuload.c
index 9e6df3fac..20b711fa6 100644
--- a/apps/systemlib/cpuload.c
+++ b/apps/systemlib/cpuload.c
@@ -79,43 +79,43 @@ void cpuload_initialize_once()
{
// if (!system_load.initialized)
// {
- system_load.start_time = hrt_absolute_time();
- int i;
- for (i = 0; i < CONFIG_MAX_TASKS; i++)
- {
- system_load.tasks[i].valid = false;
- }
- system_load.total_count = 0;
-
- uint64_t now = hrt_absolute_time();
-
- /* initialize idle thread statically */
- system_load.tasks[0].start_time = now;
- system_load.tasks[0].total_runtime = 0;
- system_load.tasks[0].curr_start_time = 0;
- system_load.tasks[0].tcb = sched_gettcb(0);
- system_load.tasks[0].valid = true;
- system_load.total_count++;
-
- /* initialize init thread statically */
- system_load.tasks[1].start_time = now;
- system_load.tasks[1].total_runtime = 0;
- system_load.tasks[1].curr_start_time = 0;
- system_load.tasks[1].tcb = sched_gettcb(1);
- system_load.tasks[1].valid = true;
- /* count init thread */
- system_load.total_count++;
- // }
+ system_load.start_time = hrt_absolute_time();
+ int i;
+
+ for (i = 0; i < CONFIG_MAX_TASKS; i++) {
+ system_load.tasks[i].valid = false;
+ }
+
+ system_load.total_count = 0;
+
+ uint64_t now = hrt_absolute_time();
+
+ /* initialize idle thread statically */
+ system_load.tasks[0].start_time = now;
+ system_load.tasks[0].total_runtime = 0;
+ system_load.tasks[0].curr_start_time = 0;
+ system_load.tasks[0].tcb = sched_gettcb(0);
+ system_load.tasks[0].valid = true;
+ system_load.total_count++;
+
+ /* initialize init thread statically */
+ system_load.tasks[1].start_time = now;
+ system_load.tasks[1].total_runtime = 0;
+ system_load.tasks[1].curr_start_time = 0;
+ system_load.tasks[1].tcb = sched_gettcb(1);
+ system_load.tasks[1].valid = true;
+ /* count init thread */
+ system_load.total_count++;
+ // }
}
-void sched_note_start(FAR _TCB *tcb )
+void sched_note_start(FAR _TCB *tcb)
{
/* search first free slot */
int i;
- for (i = 1; i < CONFIG_MAX_TASKS; i++)
- {
- if (!system_load.tasks[i].valid)
- {
+
+ for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+ if (!system_load.tasks[i].valid) {
/* slot is available */
system_load.tasks[i].start_time = hrt_absolute_time();
system_load.tasks[i].total_runtime = 0;
@@ -128,13 +128,12 @@ void sched_note_start(FAR _TCB *tcb )
}
}
-void sched_note_stop(FAR _TCB *tcb )
+void sched_note_stop(FAR _TCB *tcb)
{
int i;
- for (i = 1; i < CONFIG_MAX_TASKS; i++)
- {
- if (system_load.tasks[i].tcb->pid == tcb->pid)
- {
+
+ for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+ if (system_load.tasks[i].tcb->pid == tcb->pid) {
/* mark slot as fee */
system_load.tasks[i].valid = false;
system_load.tasks[i].total_runtime = 0;
@@ -152,26 +151,23 @@ void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
/* Kind of inefficient: find both tasks and update times */
uint8_t both_found = 0;
- for (int i = 0; i < CONFIG_MAX_TASKS; i++)
- {
+
+ for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
/* Task ending its current scheduling run */
- if (system_load.tasks[i].tcb->pid == pFromTcb->pid)
- {
+ if (system_load.tasks[i].tcb->pid == pFromTcb->pid) {
//if (system_load.tasks[i].curr_start_time != 0)
{
system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
}
both_found++;
- }
- else if (system_load.tasks[i].tcb->pid == pToTcb->pid)
- {
+
+ } else if (system_load.tasks[i].tcb->pid == pToTcb->pid) {
system_load.tasks[i].curr_start_time = new_time;
both_found++;
}
/* Do only iterate as long as needed */
- if (both_found == 2)
- {
+ if (both_found == 2) {
break;
}
}
diff --git a/apps/systemlib/err.c b/apps/systemlib/err.c
index f22c5ed0d..3011743a1 100644
--- a/apps/systemlib/err.c
+++ b/apps/systemlib/err.c
@@ -79,8 +79,10 @@ warnerr_core(int errcode, const char *fmt, va_list args)
/* convenience as many parts of NuttX use negative errno */
if (errcode < 0)
errcode = -errcode;
+
if (errcode < NOCODE)
fprintf(stderr, ": %s", strerror(errcode));
+
fprintf(stderr, "\n");
#elif CONFIG_ARCH_LOWPUTC
lib_lowprintf("%s: ", getprogname());
@@ -89,8 +91,10 @@ warnerr_core(int errcode, const char *fmt, va_list args)
/* convenience as many parts of NuttX use negative errno */
if (errcode < 0)
errcode = -errcode;
+
if (errcode < NOCODE)
lib_lowprintf(": %s", strerror(errcode));
+
lib_lowprintf("\n");
#endif
}
@@ -144,7 +148,7 @@ verrx(int exitcode, const char *fmt, va_list args)
}
void
-warn(const char *fmt, ...)
+warn(const char *fmt, ...)
{
va_list args;
@@ -153,13 +157,13 @@ warn(const char *fmt, ...)
}
void
-vwarn(const char *fmt, va_list args)
+vwarn(const char *fmt, va_list args)
{
warnerr_core(errno, fmt, args);
}
void
-warnc(int errcode, const char *fmt, ...)
+warnc(int errcode, const char *fmt, ...)
{
va_list args;
@@ -168,13 +172,13 @@ warnc(int errcode, const char *fmt, ...)
}
void
-vwarnc(int errcode, const char *fmt, va_list args)
+vwarnc(int errcode, const char *fmt, va_list args)
{
warnerr_core(errcode, fmt, args);
}
void
-warnx(const char *fmt, ...)
+warnx(const char *fmt, ...)
{
va_list args;
@@ -183,7 +187,7 @@ warnx(const char *fmt, ...)
}
void
-vwarnx(const char *fmt, va_list args)
+vwarnx(const char *fmt, va_list args)
{
warnerr_core(NOCODE, fmt, args);
}
diff --git a/apps/systemlib/err.h b/apps/systemlib/err.h
index f798b97e7..ca13d6265 100644
--- a/apps/systemlib/err.h
+++ b/apps/systemlib/err.h
@@ -36,30 +36,30 @@
*
* Simple error/warning functions, heavily inspired by the BSD functions of
* the same names.
- *
+ *
* The err() and warn() family of functions display a formatted error
* message on the standard error output. In all cases, the last
* component of the program name, a colon character, and a space are
* output. If the fmt argument is not NULL, the printf(3)-like formatted
* error message is output. The output is terminated by a newline
* character.
- *
+ *
* The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and
* vwarnc() functions append an error message obtained from strerror(3)
* based on a supplied error code value or the global variable errno,
* preceded by another colon and space unless the fmt argument is NULL.
- *
+ *
* In the case of the errc(), verrc(), warnc(), and vwarnc() functions,
* the code argument is used to look up the error message.
- *
+ *
* The err(), verr(), warn(), and vwarn() functions use the global
* variable errno to look up the error message.
- *
+ *
* The errx() and warnx() functions do not append an error message.
- *
+ *
* The err(), verr(), errc(), verrc(), errx(), and verrx() functions do
* not return, but exit with the value of the argument eval.
- *
+ *
*/
#ifndef _SYSTEMLIB_ERR_H
@@ -71,18 +71,18 @@ __BEGIN_DECLS
__EXPORT const char *getprogname(void);
-__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3)));
-__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0)));
-__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn,format(printf,3, 4)));
-__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn,format(printf,3, 0)));
-__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3)));
-__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0)));
-__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf,1, 2)));
-__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf,1, 0)));
-__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf,2, 3)));
-__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf,2, 0)));
-__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf,1, 2)));
-__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf,1, 0)));
+__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3)));
+__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0)));
+__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn, format(printf, 3, 4)));
+__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn, format(printf, 3, 0)));
+__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3)));
+__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0)));
+__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf, 1, 2)));
+__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf, 1, 0)));
+__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf, 2, 3)));
+__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf, 2, 0)));
+__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf, 1, 2)));
+__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, 0)));
__END_DECLS
diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c
index bc467bfa3..789368fbd 100644
--- a/apps/systemlib/geo/geo.c
+++ b/apps/systemlib/geo/geo.c
@@ -89,7 +89,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
{
// This function returns the distance to the nearest point on the track line. Distance is positive if current
-// position is right of the track and negative if left of the track as seen from a point on the track line
+// position is right of the track and negative if left of the track as seen from a point on the track line
// headed towards the end point.
crosstrack_error_s return_var;
@@ -97,43 +97,46 @@ __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now,
float bearing_end;
float bearing_track;
float bearing_diff;
-
+
return_var.error = true; // Set error flag, cleared when valid result calculated.
return_var.past_end = false;
return_var.distance = 0.0f;
return_var.bearing = 0.0f;
-
+
// Return error if arguments are bad
- if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var;
-
+ if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var;
+
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
bearing_diff = bearing_track - bearing_end;
bearing_diff = _wrapPI(bearing_diff);
-
+
// Return past_end = true if past end point of line
- if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
+ if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
return_var.past_end = true;
return_var.error = false;
return return_var;
}
-
+
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
- return_var.distance = (dist_to_end)*sin(bearing_diff);
- if(sin(bearing_diff) >=0 ) {
+ return_var.distance = (dist_to_end) * sin(bearing_diff);
+
+ if (sin(bearing_diff) >= 0) {
return_var.bearing = _wrapPI(bearing_track - M_PI_2_F);
+
} else {
return_var.bearing = _wrapPI(bearing_track + M_PI_2_F);
}
+
return_var.error = false;
-
+
return return_var;
}
-__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
- float radius, float arc_start_bearing, float arc_sweep)
+__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
+ float radius, float arc_start_bearing, float arc_sweep)
{
// This function returns the distance to the nearest point on the track arc. Distance is positive if current
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
@@ -146,68 +149,76 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
float bearing_sector_end;
float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
bool in_sector;
-
+
return_var.error = true; // Set error flag, cleared when valid result calculated.
return_var.past_end = false;
return_var.distance = 0.0f;
return_var.bearing = 0.0f;
-
+
// Return error if arguments are bad
- if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var;
-
-
- if(arc_sweep >= 0) {
+ if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var;
+
+
+ if (arc_sweep >= 0) {
bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep;
- if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F;
+
+ if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
+
} else {
bearing_sector_end = arc_start_bearing;
bearing_sector_start = arc_start_bearing - arc_sweep;
- if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F;
+
+ if (bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F;
}
+
in_sector = false;
-
+
// Case where sector does not span zero
- if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
-
+ if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
+
// Case where sector does span zero
- if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true;
-
+ if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
+
// If in the sector then calculate distance and bearing to closest point
- if(in_sector) {
+ if (in_sector) {
return_var.past_end = false;
float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
-
- if(dist_to_center <= radius) {
+
+ if (dist_to_center <= radius) {
return_var.distance = radius - dist_to_center;
return_var.bearing = bearing_now + M_PI_F;
+
} else {
return_var.distance = dist_to_center - radius;
return_var.bearing = bearing_now;
}
-
- // If out of the sector then calculate dist and bearing to start or end point
+
+ // If out of the sector then calculate dist and bearing to start or end point
+
} else {
-
- // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude)
- // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
- // calculate the position of the start and end points. We should not be doing this often
- // as this function generally will not be called repeatedly when we are out of the sector.
-
- // TO DO - this is messed up and won't compile
+
+ // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude)
+ // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
+ // calculate the position of the start and end points. We should not be doing this often
+ // as this function generally will not be called repeatedly when we are out of the sector.
+
+ // TO DO - this is messed up and won't compile
float start_disp_x = radius * sin(arc_start_bearing);
float start_disp_y = radius * cos(arc_start_bearing);
- float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep));
- float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep));
- float lon_start = lon_now + start_disp_x/111111.0d;
- float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d;
- float lon_end = lon_now + end_disp_x/111111.0d;
- float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d;
+ float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
+ float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
+ float lon_start = lon_now + start_disp_x / 111111.0d;
+ float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
+ float lon_end = lon_now + end_disp_x / 111111.0d;
+ float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
- if(dist_to_start < dist_to_end) {
+
+ if (dist_to_start < dist_to_end) {
return_var.distance = dist_to_start;
return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+
} else {
return_var.past_end = true;
return_var.distance = dist_to_end;
@@ -215,10 +226,10 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
}
}
-
+
return_var.bearing = _wrapPI(return_var.bearing);
return_var.error = false;
- return return_var;
+ return return_var;
}
float _wrapPI(float bearing)
@@ -227,21 +238,25 @@ float _wrapPI(float bearing)
while (bearing > M_PI_F) {
bearing = bearing - M_TWOPI_F;
}
+
while (bearing <= -M_PI_F) {
bearing = bearing + M_TWOPI_F;
}
+
return bearing;
}
-
+
float _wrap2PI(float bearing)
{
while (bearing >= M_TWOPI_F) {
bearing = bearing - M_TWOPI_F;
}
+
while (bearing < 0.0f) {
bearing = bearing + M_TWOPI_F;
}
+
return bearing;
}
@@ -251,23 +266,26 @@ float _wrap180(float bearing)
while (bearing > 180.0f) {
bearing = bearing - 360.0f;
}
+
while (bearing <= -180.0f) {
bearing = bearing + 360.0f;
}
+
return bearing;
}
-
+
float _wrap360(float bearing)
{
while (bearing >= 360.0f) {
bearing = bearing - 360.0f;
}
+
while (bearing < 0.0f) {
bearing = bearing + 360.0f;
}
+
return bearing;
}
-
- \ No newline at end of file
+
diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h
index 205afc79e..c98b4c85d 100644
--- a/apps/systemlib/geo/geo.h
+++ b/apps/systemlib/geo/geo.h
@@ -44,8 +44,8 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
*/
-
-
+
+
#include <stdbool.h>
typedef struct {
@@ -59,14 +59,14 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
-//
+//
__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
-__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
- float radius, float arc_start_bearing, float arc_sweep);
-
+__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
+ float radius, float arc_start_bearing, float arc_sweep);
+
float _wrap180(float bearing);
-float _wrap360(float bearing);
+float _wrap360(float bearing);
float _wrapPI(float bearing);
float _wrap2PI(float bearing); \ No newline at end of file
diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp
index b56226c03..6c1bbe469 100644
--- a/apps/systemlib/mixer/mixer.cpp
+++ b/apps/systemlib/mixer/mixer.cpp
@@ -63,7 +63,7 @@ float
Mixer::get_control(uint8_t group, uint8_t index)
{
float value;
-
+
_control_cb(_cb_handle, group, index, value);
return value;
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp
index 44a33bf8b..004151235 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/apps/systemlib/mixer/mixer_group.cpp
@@ -224,29 +224,35 @@ mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, co
if (!strcmp(geomname, "4+")) {
geometry = MultirotorMixer::QUAD_PLUS;
+
} else if (!strcmp(geomname, "4x")) {
geometry = MultirotorMixer::QUAD_X;
+
} else if (!strcmp(geomname, "6+")) {
geometry = MultirotorMixer::HEX_PLUS;
+
} else if (!strcmp(geomname, "6x")) {
geometry = MultirotorMixer::HEX_X;
+
} else if (!strcmp(geomname, "8+")) {
geometry = MultirotorMixer::OCTA_PLUS;
+
} else if (!strcmp(geomname, "8x")) {
geometry = MultirotorMixer::OCTA_X;
+
} else {
debug("unrecognised geometry '%s'", geomname);
return nullptr;
}
return new MultirotorMixer(
- control_cb,
- cb_handle,
- geometry,
- s[0] / 10000.0f,
- s[1] / 10000.0f,
- s[2] / 10000.0f,
- s[3] / 10000.0f);
+ control_cb,
+ cb_handle,
+ geometry,
+ s[0] / 10000.0f,
+ s[1] / 10000.0f,
+ s[2] / 10000.0f,
+ s[3] / 10000.0f);
}
int
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp
index e2576a848..0e714ed50 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/apps/systemlib/mixer/mixer_multirotor.cpp
@@ -57,7 +57,7 @@
#define CW (-1.0f)
#define CCW (1.0f)
-namespace
+namespace
{
/*
@@ -167,17 +167,21 @@ MultirotorMixer::mix(float *outputs, unsigned space)
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust;
+
if (tmp > max)
max = tmp;
+
outputs[i] = tmp;
}
/* scale values into the -1.0 - 1.0 range */
if (max > 1.0f) {
fixup_scale = 2.0f / max;
+
} else {
fixup_scale = 2.0f;
}
+
for (unsigned i = 0; i < _rotor_count; i++)
outputs[i] = -1.0f + (outputs[i] * fixup_scale);
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index 6dacb1635..eeb867f11 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -189,6 +189,7 @@ param_notify_changes(void)
*/
if (param_topic == -1) {
param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
+
} else {
orb_publish(ORB_ID(parameter_update), param_topic, &pup);
}
@@ -455,6 +456,7 @@ param_reset(param_t param)
utarray_erase(param_values, pos, 1);
}
}
+
param_unlock();
if (s != NULL)
@@ -560,8 +562,7 @@ out:
return result;
}
-struct param_import_state
-{
+struct param_import_state {
bool mark_saved;
};
@@ -689,9 +690,10 @@ param_import_internal(int fd, bool mark_saved)
do {
result = bson_decoder_next(&decoder);
- } while(result > 0);
+ } while (result > 0);
out:
+
if (result < 0)
debug("BSON error decoding parameters");
diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c
index f9b50d030..7e277cdc7 100644
--- a/apps/systemlib/pid/pid.c
+++ b/apps/systemlib/pid/pid.c
@@ -43,7 +43,7 @@
#include <math.h>
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
- float limit, uint8_t mode)
+ float limit, uint8_t mode)
{
pid->kp = kp;
pid->ki = ki;
@@ -65,30 +65,35 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
if (isfinite(kp)) {
pid->kp = kp;
+
} else {
ret = 1;
}
if (isfinite(ki)) {
pid->ki = ki;
+
} else {
ret = 1;
}
if (isfinite(kd)) {
pid->kd = kd;
+
} else {
ret = 1;
}
if (isfinite(intmax)) {
pid->intmax = intmax;
+
} else {
ret = 1;
}
-
+
if (isfinite(limit)) {
pid->limit = limit;
+
} else {
ret = 1;
}
@@ -121,17 +126,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
goto start
*/
- if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt))
- {
+ if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
return pid->last_output;
}
float i, d;
pid->sp = sp;
-
+
// Calculated current error value
float error = pid->sp - val;
-
+
if (isfinite(error)) { // Why is this necessary? DEW
pid->error_previous = error;
}
@@ -140,30 +144,36 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / dt;
+
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
d = -val_dot;
+
} else {
d = 0.0f;
}
// Calculate the error integral and check for saturation
i = pid->integral + (error * dt);
- if( fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
- fabs(i) > pid->intmax )
- {
+
+ if (fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
+ fabs(i) > pid->intmax) {
i = pid->integral; // If saturated then do not update integral value
pid->saturated = 1;
+
} else {
if (!isfinite(i)) {
i = 0;
}
+
pid->integral = i;
pid->saturated = 0;
}
// Calculate the output. Limit output magnitude to pid->limit
float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+
if (output > pid->limit) output = pid->limit;
+
if (output < -pid->limit) output = -pid->limit;
if (isfinite(output)) {
diff --git a/apps/systemlib/ppm_decode.c b/apps/systemlib/ppm_decode.c
index dd6d43a77..a5d2f738d 100644
--- a/apps/systemlib/ppm_decode.c
+++ b/apps/systemlib/ppm_decode.c
@@ -30,7 +30,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
-
+
/**
* @file ppm_decode.c
*
@@ -51,7 +51,7 @@
*
* The PPM decoder works as follows.
*
- * Initially, the decoder waits in the UNSYNCH state for two edges
+ * Initially, the decoder waits in the UNSYNCH state for two edges
* separated by PPM_MIN_START. Once the second edge is detected,
* the decoder moves to the ARM state.
*
@@ -64,9 +64,9 @@
*
* The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when
* received calculates the time from the previous mark and records
- * this time as the value for the next channel.
+ * this time as the value for the next channel.
*
- * If at any time waiting for an edge, the delay from the previous edge
+ * If at any time waiting for an edge, the delay from the previous edge
* exceeds PPM_MIN_START the frame is deemed to have ended and the recorded
* values are advertised to clients.
*/
@@ -132,21 +132,23 @@ ppm_input_decode(bool reset, unsigned count)
/* how long since the last edge? */
width = count - ppm.last_edge;
+
if (count < ppm.last_edge)
width += ppm.count_max; /* handle wrapped count */
+
ppm.last_edge = count;
- /*
+ /*
* If this looks like a start pulse, then push the last set of values
* and reset the state machine.
*
* Note that this is not a "high performance" design; it implies a whole
- * frame of latency between the pulses being received and their being
+ * frame of latency between the pulses being received and their being
* considered valid.
*/
if (width >= PPM_MIN_START) {
- /*
+ /*
* If the number of channels changes unexpectedly, we don't want
* to just immediately jump on the new count as it may be a result
* of noise or dropped edges. Instead, take a few frames to settle.
@@ -169,11 +171,13 @@ ppm_input_decode(bool reset, unsigned count)
ppm_decoded_channels = new_channel_count;
new_channel_count = 0;
}
+
} else {
/* frame channel count matches expected, let's use it */
if (ppm.next_channel > PPM_MIN_CHANNELS) {
for (i = 0; i < ppm.next_channel; i++)
ppm_buffer[i] = ppm_temp_buffer[i];
+
ppm_last_valid_decode = hrt_absolute_time();
}
}
@@ -193,10 +197,11 @@ ppm_input_decode(bool reset, unsigned count)
return;
case ARM:
+
/* we expect a pulse giving us the first mark */
if (width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too long */
-
+
/* record the mark timing, expect an inactive edge */
ppm.last_mark = count;
ppm.phase = INACTIVE;
@@ -211,6 +216,7 @@ ppm_input_decode(bool reset, unsigned count)
return;
case ACTIVE:
+
/* we expect a well-formed pulse */
if (width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too long */
@@ -228,7 +234,7 @@ ppm_input_decode(bool reset, unsigned count)
ppm_temp_buffer[ppm.next_channel++] = interval;
ppm.phase = INACTIVE;
- return;
+ return;
}
diff --git a/apps/systemlib/ppm_decode.h b/apps/systemlib/ppm_decode.h
index 681cbe830..c2b24321a 100644
--- a/apps/systemlib/ppm_decode.h
+++ b/apps/systemlib/ppm_decode.h
@@ -30,7 +30,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
-
+
/**
* @file ppm_decode.h
*
diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c
index 94a5283f0..50ac62464 100644
--- a/apps/systemlib/systemlib.c
+++ b/apps/systemlib/systemlib.c
@@ -131,7 +131,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma
pid = task_create(name, priority, stack_size, entry, argv);
if (pid > 0) {
-
+
/* configure the scheduler */
struct sched_param param;
@@ -140,6 +140,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma
/* XXX do any other private task accounting here before the task starts */
}
+
sched_unlock();
return pid;
@@ -157,15 +158,18 @@ int fmu_get_board_info(struct fmu_board_info_s *info)
statres = stat("/dev/bma180", &sb);
if (statres == OK) {
- /* BMA180 indicates a v1.5-v1.6 board */
- strcpy(info->board_name, "FMU v1.6");
- info->board_version = 16;
+ /* BMA180 indicates a v1.5-v1.6 board */
+ strcpy(info->board_name, "FMU v1.6");
+ info->board_version = 16;
+
} else {
statres = stat("/dev/accel", &sb);
+
if (statres == OK) {
/* MPU-6000 indicates a v1.7+ board */
strcpy(info->board_name, "FMU v1.7");
info->board_version = 17;
+
} else {
/* If no BMA and no MPU is present, it is a v1.3 board */
strcpy(info->board_name, "FMU v1.3");
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-03 17:20:33 +0000