diff options
Diffstat (limited to 'apps/systemlib')
-rw-r--r-- | apps/systemlib/bson/tinybson.c | 24 | ||||
-rw-r--r-- | apps/systemlib/bson/tinybson.h | 27 | ||||
-rw-r--r-- | apps/systemlib/conversions.c | 14 | ||||
-rw-r--r-- | apps/systemlib/cpuload.c | 88 | ||||
-rw-r--r-- | apps/systemlib/err.c | 16 | ||||
-rw-r--r-- | apps/systemlib/err.h | 38 | ||||
-rw-r--r-- | apps/systemlib/geo/geo.c | 120 | ||||
-rw-r--r-- | apps/systemlib/geo/geo.h | 14 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer.cpp | 2 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_group.cpp | 20 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_multirotor.cpp | 6 | ||||
-rw-r--r-- | apps/systemlib/param/param.c | 8 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.c | 28 | ||||
-rw-r--r-- | apps/systemlib/ppm_decode.c | 24 | ||||
-rw-r--r-- | apps/systemlib/ppm_decode.h | 2 | ||||
-rw-r--r-- | apps/systemlib/systemlib.c | 12 |
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"); |