dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of github.com:PX4/Firmware into power_enforce

This commit is contained in:
Lorenz Meier 2014-06-30 10:04:34 +02:00
parent 154f14e747 857b843d44
commit 6aed623b6c
45 changed files with 163 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/boards/px4fmu-v2/px4fmu2_led.c
View File

@ -54,7 +54,7 @@
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);

4
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -364,7 +364,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %d pa", report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@ -389,7 +389,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("diff pressure: %d pa", report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
}
/* reset the sensor polling to its default rate */

6
src/drivers/gps/gps.cpp
View File

@ -357,7 +357,7 @@ GPS::task_main()
}
if (!_healthy) {
char *mode_str = "unknown";
const char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
@ -449,7 +449,7 @@ GPS::print_info()
if (_report.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
@ -578,7 +578,7 @@ gps_main(int argc, char *argv[])
{
/* set to default */
char *device_name = GPS_DEFAULT_UART_PORT;
const char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false;
/*

4
src/drivers/hmc5883/hmc5883.cpp
View File

@ -326,9 +326,9 @@ HMC5883::HMC5883(int bus) :
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_collect_phase(false),
_class_instance(-1),
_mag_topic(-1),
_subsystem_pub(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@ -1228,7 +1228,7 @@ HMC5883::print_info()
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
(double)1.0/_range_scale, (double)_range_ga);
(double)(1.0f/_range_scale), (double)_range_ga);
_reports->print_info("report queue");
}

21
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -222,15 +222,15 @@ MK::MK(int bus, const char *_device_path) :
_task(-1),
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_esc_status(0),
_num_outputs(0),
_motortest(false),
_overrideSecurityChecks(false),
_motor(-1),
_px4mode(MAPPING_MK),
_frametype(FRAME_PLUS),
_t_outputs(0),
_t_esc_status(0),
_num_outputs(0),
_primary_pwm_device(false),
_motortest(false),
_overrideSecurityChecks(false),
_task_should_exit(false),
_armed(false),
_mixers(nullptr)
@ -440,9 +440,6 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
void
MK::task_main()
{
long update_rate_in_us = 0;
float tmpVal = 0;
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
@ -483,7 +480,6 @@ MK::task_main()
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
update_rate_in_us = long(1000000 / _update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
@ -735,7 +731,6 @@ MK::mk_servo_set(unsigned int chan, short val)
_retries = 0;
uint8_t result[3] = { 0, 0, 0 };
uint8_t msg[2] = { 0, 0 };
uint8_t rod = 0;
uint8_t bytesToSendBL2 = 2;
tmpVal = val;
@ -824,7 +819,7 @@ MK::mk_servo_set(unsigned int chan, short val)
if (debugCounter == 2000) {
debugCounter = 0;
for (int i = 0; i < _num_outputs; i++) {
for (unsigned int i = 0; i < _num_outputs; i++) {
if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
}
@ -1169,7 +1164,7 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr
}
int
mk_start(unsigned motors, char *device_path)
mk_start(unsigned motors, const char *device_path)
{
int ret;
@ -1228,7 +1223,7 @@ mkblctrl_main(int argc, char *argv[])
bool overrideSecurityChecks = false;
bool showHelp = false;
bool newMode = false;
char *devicepath = "";
const char *devicepath = "";
/*
* optional parameters

12
src/drivers/px4fmu/fmu.cpp
View File

@ -240,8 +240,6 @@ PX4FMU::PX4FMU() :
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_task(-1),
_control_subs({-1}),
_poll_fds_num(0),
_armed_sub(-1),
_outputs_pub(-1),
_num_outputs(0),
@ -252,10 +250,12 @@ PX4FMU::PX4FMU() :
_mixers(nullptr),
_groups_required(0),
_groups_subscribed(0),
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
_control_subs{-1},
_poll_fds_num(0),
_failsafe_pwm{0},
_disarmed_pwm{0},
_num_failsafe_set(0),
_num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;

2
src/drivers/px4io/px4io_serial.cpp
View File

@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt()
if (_rx_dma_status == _dma_status_waiting) {
/* verify that the received packet is complete */
int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
perf_count(_pc_badidle);

2
src/drivers/sf0x/sf0x.cpp
View File

@ -616,7 +616,7 @@ SF0X::collect()
}
}
debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO"));
debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
/* done with this chunk, resetting - even if invalid */
_linebuf_index = 0;

1
src/examples/px4_simple_app/px4_simple_app.c
View File

@ -41,6 +41,7 @@
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>

33
src/lib/geo/geo.c
View File

@ -82,8 +82,8 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
{
float x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
float y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
@ -146,7 +146,6 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
@ -174,8 +173,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
*lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
*lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
}
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
@ -197,7 +196,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@ -212,7 +211,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
}
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
if (sin(bearing_diff) >= 0) {
crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
@ -248,7 +247,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; }
if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
if (arc_sweep >= 0) {
@ -296,14 +295,14 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
// as this function generally will not be called repeatedly when we are out of the sector.
// TO DO - this is messed up and won't compile
float start_disp_x = radius * sin(arc_start_bearing);
float start_disp_y = radius * cos(arc_start_bearing);
float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
float lon_start = lon_now + start_disp_x / 111111.0d;
float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
float lon_end = lon_now + end_disp_x / 111111.0d;
float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
float start_disp_x = radius * sinf(arc_start_bearing);
float start_disp_y = radius * cosf(arc_start_bearing);
float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep));
float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep));
float lon_start = lon_now + start_disp_x / 111111.0f;
float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f;
float lon_end = lon_now + end_disp_x / 111111.0f;
float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f;
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
@ -337,7 +336,7 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now
double d_lat = x_rad - current_x_rad;
double d_lon = y_rad - current_y_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
float dxy = CONSTANTS_RADIUS_OF_EARTH * c;

31
src/lib/geo_lookup/geo_mag_declination.c
View File

@ -54,24 +54,19 @@
static const int8_t declination_table[13][37] = \
{
46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \
-66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \
-3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \
29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \
-40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \
8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \
10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \
-10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \
-14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \
9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \
7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \
0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \
0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \
0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \
-16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \
12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \
0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \
13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3
{ 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 },
{ 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 },
{ 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 },
{ 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 },
{ 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 },
{ 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 },
{ 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 },
{ 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 },
{ 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 },
{ 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 },
{ 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 },
{ 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 },
{ 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 },
};
static float get_lookup_table_val(unsigned lat, unsigned lon);

2
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
View File

@ -101,7 +101,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->r3, &(p->r[3]));
param_get(h->mag_decl, &(p->mag_decl));
p->mag_decl *= M_PI / 180.0f;
p->mag_decl *= M_PI_F / 180.0f;
param_get(h->acc_comp, &(p->acc_comp));

21
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
View File

@ -392,8 +392,6 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
*/
int attitude_estimator_so3_thread_main(int argc, char *argv[])
{
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Time constant
float dt = 0.005f;
@ -438,11 +436,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
/* keep track of sensor updates */
@ -513,7 +509,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]);
}
} else {
@ -523,12 +519,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
@ -538,8 +531,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* update accelerometer measurements */
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
}
@ -549,8 +540,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* update magnetometer measurements */
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
@ -569,8 +558,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
continue;
}
uint64_t timing_start = hrt_absolute_time();
// NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
@ -609,9 +596,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* due to inputs or numerical failure the output is invalid, skip it */
// Due to inputs or numerical failure the output is invalid
warnx("infinite euler angles, rotation matrix:");
warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]);
warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]);
warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]);
// Don't publish anything
continue;
}

6
src/modules/commander/accelerometer_calibration.cpp
View File

@ -158,6 +158,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
int do_accel_calibration(int mavlink_fd)
{
int fd;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
struct accel_scale accel_scale = {
@ -172,7 +174,7 @@ int do_accel_calibration(int mavlink_fd)
int res = OK;
/* reset all offsets to zero and all scales to one */
int fd = open(ACCEL_DEVICE_PATH, 0);
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
@ -223,7 +225,7 @@ int do_accel_calibration(int mavlink_fd)
if (res == OK) {
/* apply new scaling and offsets */
int fd = open(ACCEL_DEVICE_PATH, 0);
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);

2
src/modules/commander/calibration_routines.cpp
View File

@ -170,7 +170,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
//Iterate N times, ignore stop condition.
int n = 0;
unsigned int n = 0;
while (n < max_iterations) {
n++;

3
src/modules/dataman/dataman.c
View File

@ -50,6 +50,7 @@
#include <string.h>
#include "dataman.h"
#include <systemlib/param/param.h>
/**
* data manager app start / stop handling function
@ -187,7 +188,7 @@ create_work_item(void)
if (item) {
item->first = 1;
lock_queue(&g_free_q);
for (int i = 1; i < k_work_item_allocation_chunk_size; i++) {
for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) {
(item + i)->first = 0;
sq_addfirst(&(item + i)->link, &(g_free_q.q));
}

2
src/modules/ekf_att_pos_estimator/estimator_23states.h
View File

@ -234,7 +234,7 @@ int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);

16
src/modules/fw_pos_control_l1/landingslope.cpp
View File

@ -46,16 +46,16 @@
#include <unistd.h>
#include <mathlib/mathlib.h>
void Landingslope::update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_relative_alt,
float H1_virt)
void Landingslope::update(float landing_slope_angle_rad_new,
float flare_relative_alt_new,
float motor_lim_relative_alt_new,
float H1_virt_new)
{
_landing_slope_angle_rad = landing_slope_angle_rad;
_flare_relative_alt = flare_relative_alt;
_motor_lim_relative_alt = motor_lim_relative_alt;
_H1_virt = H1_virt;
_landing_slope_angle_rad = landing_slope_angle_rad_new;
_flare_relative_alt = flare_relative_alt_new;
_motor_lim_relative_alt = motor_lim_relative_alt_new;
_H1_virt = H1_virt_new;
calculateSlopeValues();
}

8
src/modules/fw_pos_control_l1/landingslope.h
View File

@ -123,10 +123,10 @@ public:
float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
void update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_relative_alt,
float H1_virt);
void update(float landing_slope_angle_rad_new,
float flare_relative_alt_new,
float motor_lim_relative_alt_new,
float H1_virt_new);
inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}

6
src/modules/mavlink/mavlink_main.cpp
View File

@ -2266,13 +2266,13 @@ Mavlink::start(int argc, char *argv[])
}
void
Mavlink::status()
Mavlink::display_status()
{
warnx("running");
}
int
Mavlink::stream(int argc, char *argv[])
Mavlink::stream_command(int argc, char *argv[])
{
const char *device_name = DEFAULT_DEVICE_NAME;
float rate = -1.0f;
@ -2360,7 +2360,7 @@ int mavlink_main(int argc, char *argv[])
// mavlink::g_mavlink->status();
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream(argc, argv);
return Mavlink::stream_command(argc, argv);
} else {
usage();

4
src/modules/mavlink/mavlink_main.h
View File

@ -123,9 +123,9 @@ public:
/**
* Display the mavlink status.
*/
void status();
void display_status();
static int stream(int argc, char *argv[]);
static int stream_command(int argc, char *argv[]);
static int instance_count();

6
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -47,10 +47,10 @@
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
_fd(orb_subscribe(_topic)),
_published(false),
next(nullptr),
_topic(topic),
next(nullptr)
_fd(orb_subscribe(_topic)),
_published(false)
{
}

6
src/modules/mavlink/mavlink_stream.cpp
View File

@ -43,7 +43,11 @@
#include "mavlink_stream.h"
#include "mavlink_main.h"
MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
MavlinkStream::MavlinkStream() :
_last_sent(0),
_channel(MAVLINK_COMM_0),
_interval(1000000),
next(nullptr)
{
}

3
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -545,7 +545,6 @@ MulticopterPositionControl::task_main()
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
math::Vector<3> sp_move_rate;
sp_move_rate.zero();
@ -862,7 +861,7 @@ MulticopterPositionControl::task_main()
if (_control_mode.flag_control_velocity_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();

41
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -179,15 +179,21 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1);
}
void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
{
FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) {
char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n",
hrt_absolute_time(), msg, (double)dt,
(double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1],
(double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]);
fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n",
(double)acc[0], (double)acc[1], (double)acc[2],
(double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1],
(double)w_xy_gps_p, (double)w_xy_gps_v);
fwrite(s, 1, n, f);
free(s);
}
@ -261,9 +267,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
hrt_abstime t_prev = 0;
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
@ -285,7 +288,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime xy_src_time = 0; // time of last available position data
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
@ -370,8 +372,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
warnx("baro offs: %.2f", baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
warnx("baro offs: %.2f", (double)baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
@ -475,7 +477,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
flow_prev = flow.flow_timestamp;
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && flow.ground_distance_m != sonar_prev) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
@ -497,7 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset);
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
}
} else {
@ -510,7 +512,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) {
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
/* distance to surface */
float flow_dist = dist_bottom / att.R[2][2];
/* check if flow if too large for accurate measurements */
@ -558,7 +560,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* under ideal conditions, on 1m distance assume EPH = 10cm */
eph_flow = 0.1 / w_flow;
eph_flow = 0.1f / w_flow;
flow_valid = true;
@ -661,8 +663,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* initialize projection */
map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
}
}
@ -746,10 +748,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* increase EPH/EPV on each step */
if (eph < max_eph_epv) {
eph *= 1.0 + dt;
eph *= 1.0f + dt;
}
if (epv < max_eph_epv) {
epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift)
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
}
/* use GPS if it's valid and reference position initialized */
@ -758,11 +760,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
/* try to estimate position during some time after position sources lost */
if (use_gps_xy || use_flow) {
xy_src_time = t;
}
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);

2
src/modules/px4iofirmware/protocol.h
View File

@ -312,7 +312,7 @@ struct IOPacket {
#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK)
#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))))
static const uint8_t crc8_tab[256] __attribute__((unused)) =
{

5
src/modules/px4iofirmware/px4io.c
View File

@ -37,6 +37,7 @@
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <stdio.h> // required for task_create
#include <stdbool.h>
@ -303,14 +304,12 @@ user_start(int argc, char *argv[])
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
struct mallinfo minfo = mallinfo();
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)minfo.mxordblk);
(unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
}

3
src/modules/px4iofirmware/registers.c
View File

@ -119,7 +119,6 @@ uint16_t r_page_raw_rc_input[] =
[PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_FRAME_COUNT] = 0,
[PX4IO_P_RAW_LOST_FRAME_COUNT] = 0,
[PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
};
@ -670,7 +669,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
} else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}

2
src/modules/sensors/sensors.cpp
View File

@ -649,7 +649,7 @@ Sensors::parameters_update()
if (!isfinite(tmpScaleFactor) ||
(tmpRevFactor < 0.000001f) ||
(tmpRevFactor > 0.2f)) {
warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i]));
/* scaling factors do not make sense, lock them down */
_parameters.scaling_factor[i] = 0.0f;
rc_valid = false;

2
src/modules/systemlib/hx_stream.c
View File

@ -63,7 +63,7 @@ struct hx_stream {
/* TX state */
int fd;
bool tx_error;
uint8_t *tx_buf;
const uint8_t *tx_buf;
unsigned tx_resid;
uint32_t tx_crc;
enum {

4
src/modules/systemlib/mixer/mixer_multirotor.cpp
View File

@ -208,7 +208,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
char geomname[8];
int s[4];
int used;
const char *end = buf + buflen;
/* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) {
@ -302,7 +301,6 @@ MultirotorMixer::mix(float *outputs, unsigned space)
//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
float min_out = 0.0f;
float max_out = 0.0f;
float scale_yaw = 1.0f;
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
@ -327,7 +325,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
}
/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
if (min_out < 0.0) {
if (min_out < 0.0f) {
float scale_in = thrust / (thrust - min_out);
/* mix again with adjusted controls */

2
src/modules/systemlib/otp.c
View File

@ -133,7 +133,7 @@ int lock_otp(void)
// COMPLETE, BUSY, or other flash error?
int F_GetStatus(void)
static int F_GetStatus(void)
{
int fs = F_COMPLETE;

2
src/modules/systemlib/param/param.c
View File

@ -96,8 +96,6 @@ ORB_DEFINE(parameter_update, struct parameter_update_s);
/** parameter update topic handle */
static orb_advert_t param_topic = -1;
static sem_t param_sem = { .semcount = 1 };
/** lock the parameter store */
static void
param_lock(void)

1
src/modules/systemlib/pwm_limit/pwm_limit.c
View File

@ -97,7 +97,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
}
unsigned progress;
uint16_t temp_pwm;
/* then set effective_pwm based on state */
switch (limit->state) {

30
src/systemcmds/mtd/mtd.c
View File

@ -91,7 +91,7 @@ static void mtd_test(void);
static void mtd_erase(char *partition_names[], unsigned n_partitions);
static void mtd_readtest(char *partition_names[], unsigned n_partitions);
static void mtd_rwtest(char *partition_names[], unsigned n_partitions);
static void mtd_print_info();
static void mtd_print_info(void);
static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
@ -104,6 +104,16 @@ static unsigned n_partitions_current = 0;
static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"};
static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]);
static void
mtd_status(void)
{
if (!attached)
errx(1, "MTD driver not started");
mtd_print_info();
exit(0);
}
int mtd_main(int argc, char *argv[])
{
if (argc >= 2) {
@ -355,7 +365,7 @@ static ssize_t mtd_get_partition_size(void)
return partsize;
}
void mtd_print_info()
void mtd_print_info(void)
{
if (!attached)
exit(1);
@ -385,16 +395,6 @@ mtd_test(void)
exit(1);
}
void
mtd_status(void)
{
if (!attached)
errx(1, "MTD driver not started");
mtd_print_info();
exit(0);
}
void
mtd_erase(char *partition_names[], unsigned n_partitions)
{
@ -428,7 +428,7 @@ mtd_readtest(char *partition_names[], unsigned n_partitions)
uint8_t v[128];
for (uint8_t i = 0; i < n_partitions; i++) {
uint32_t count = 0;
ssize_t count = 0;
printf("reading %s expecting %u bytes\n", partition_names[i], expected_size);
int fd = open(partition_names[i], O_RDONLY);
if (fd == -1) {
@ -459,8 +459,8 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions)
uint8_t v[128], v2[128];
for (uint8_t i = 0; i < n_partitions; i++) {
uint32_t count = 0;
off_t offset = 0;
ssize_t count = 0;
off_t offset = 0;
printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size);
int fd = open(partition_names[i], O_RDWR);
if (fd == -1) {

4
src/systemcmds/param/param.c
View File

@ -63,7 +63,7 @@ static void do_show(const char* search_string);
static void do_show_print(void *arg, param_t param);
static void do_set(const char* name, const char* val, bool fail_on_not_found);
static void do_compare(const char* name, const char* vals[], unsigned comparisons);
static void do_reset();
static void do_reset(void);
int
param_main(int argc, char *argv[])
@ -416,7 +416,7 @@ do_compare(const char* name, const char* vals[], unsigned comparisons)
}
static void
do_reset()
do_reset(void)
{
param_reset_all();

2
src/systemcmds/tests/test_conv.cpp
View File

@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[])
float f = i/10000.0f;
float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
if (fabsf(f - fres) > 0.0001f) {
warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres);
return 1;
}
}

7
src/systemcmds/tests/test_file.c
View File

@ -102,7 +102,7 @@ test_file(int argc, char *argv[])
}
/* perform tests for a range of chunk sizes */
unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
int chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
@ -116,7 +116,7 @@ test_file(int argc, char *argv[])
uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
/* fill write buffer with known values */
for (int i = 0; i < sizeof(write_buf); i++) {
for (size_t i = 0; i < sizeof(write_buf); i++) {
/* this will wrap, but we just need a known value with spacing */
write_buf[i] = i+11;
}
@ -224,9 +224,6 @@ test_file(int argc, char *argv[])
return 1;
}
/* compare value */
bool compare_ok = true;
for (int j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j]) {
warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]);

10
src/systemcmds/tests/test_file2.c
View File

@ -49,6 +49,8 @@
#include <stdlib.h>
#include <getopt.h>
#include "tests.h"
#define FLAG_FSYNC 1
#define FLAG_LSEEK 2
@ -85,9 +87,9 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t
buffer[j] = get_value(ofs);
ofs++;
}
if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
printf("write failed at offset %u\n", ofs);
exit(1);
if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
printf("write failed at offset %u\n", ofs);
exit(1);
}
if (flags & FLAG_FSYNC) {
fsync(fd);
@ -116,7 +118,7 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t
printf("read ofs=%u\r", ofs);
}
counter++;
if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
printf("read failed at offset %u\n", ofs);
exit(1);
}

10
src/systemcmds/tests/test_float.c
View File

@ -94,7 +94,7 @@ int test_float(int argc, char *argv[])
printf("\t success: asinf(1.0f) == 1.57079f\n");
} else {
printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one);
printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one);
ret = -1;
}
@ -128,7 +128,7 @@ int test_float(int argc, char *argv[])
float sinf_zero_one = sinf(0.1f);
if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
printf("\t success: sinf(0.1f) == 0.09983f\n");
} else {
@ -155,7 +155,7 @@ int test_float(int argc, char *argv[])
}
char sbuf[30];
sprintf(sbuf, "%8.4f", 0.553415f);
sprintf(sbuf, "%8.4f", (double)0.553415f);
if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
@ -166,7 +166,7 @@ int test_float(int argc, char *argv[])
ret = -5;
}
sprintf(sbuf, "%8.4f", -0.553415f);
sprintf(sbuf, "%8.4f", (double)-0.553415f);
if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
@ -205,7 +205,7 @@ int test_float(int argc, char *argv[])
printf("\t success: (float) 1.55f == 1.55 (double)\n");
} else {
printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1);
printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1);
ret = -8;
}

4
src/systemcmds/tests/test_mathlib.cpp
View File

@ -52,10 +52,6 @@
using namespace math;
const char* formatResult(bool res) {
return res ? "OK" : "ERROR";
}
int test_mathlib(int argc, char *argv[])
{
warnx("testing mathlib");

33
src/systemcmds/tests/test_mixer.cpp
View File

@ -81,7 +81,7 @@ int test_mixer(int argc, char *argv[])
warnx("testing mixer");
char *filename = "/etc/mixers/IO_pass.mix";
const char *filename = "/etc/mixers/IO_pass.mix";
if (argc > 1)
filename = argv[1];
@ -100,8 +100,6 @@ int test_mixer(int argc, char *argv[])
* e.g. on PX4IO.
*/
unsigned nused = 0;
const unsigned chunk_size = 64;
MixerGroup mixer_group(mixer_callback, 0);
@ -124,7 +122,6 @@ int test_mixer(int argc, char *argv[])
return 1;
/* FIRST mark the mixer as invalid */
bool mixer_ok = false;
/* THEN actually delete it */
mixer_group.reset();
char mixer_text[256]; /* large enough for one mixer */
@ -140,7 +137,6 @@ int test_mixer(int argc, char *argv[])
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
bool mixer_ok = false;
return 1;
}
@ -156,15 +152,6 @@ int test_mixer(int argc, char *argv[])
/* if anything was parsed */
if (resid != mixer_text_length) {
/* only set mixer ok if no residual is left over */
if (resid == 0) {
mixer_ok = true;
} else {
/* not yet reached the end of the mixer, set as not ok */
mixer_ok = false;
}
warnx("used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
@ -192,7 +179,7 @@ int test_mixer(int argc, char *argv[])
should_arm = true;
/* run through arming phase */
for (int i = 0; i < output_max; i++) {
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
@ -213,7 +200,7 @@ int test_mixer(int argc, char *argv[])
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int i = 0; i < mixed; i++)
for (unsigned i = 0; i < mixed; i++)
{
/* check mixed outputs to be zero during init phase */
if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
@ -228,7 +215,7 @@ int test_mixer(int argc, char *argv[])
return 1;
}
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]);
}
usleep(sleep_quantum_us);
sleepcount++;
@ -244,7 +231,7 @@ int test_mixer(int argc, char *argv[])
for (int j = -jmax; j <= jmax; j++) {
for (int i = 0; i < output_max; i++) {
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = j/10.0f + 0.1f * i;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
@ -257,11 +244,11 @@ int test_mixer(int argc, char *argv[])
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
warnx("mixed %d outputs (max %d)", mixed, output_max);
for (int i = 0; i < mixed; i++)
for (unsigned i = 0; i < mixed; i++)
{
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
warnx("mixer violated predicted value");
return 1;
}
@ -282,7 +269,7 @@ int test_mixer(int argc, char *argv[])
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int i = 0; i < mixed; i++)
for (unsigned i = 0; i < mixed; i++)
{
/* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
@ -316,7 +303,7 @@ int test_mixer(int argc, char *argv[])
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int i = 0; i < mixed; i++)
for (unsigned i = 0; i < mixed; i++)
{
/* predict value */
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
@ -333,7 +320,7 @@ int test_mixer(int argc, char *argv[])
/* check post ramp phase */
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
warnx("mixer violated predicted value");
return 1;
}

1
src/systemcmds/tests/test_ppm_loopback.c
View File

@ -65,7 +65,6 @@ int test_ppm_loopback(int argc, char *argv[])
int _rc_sub = orb_subscribe(ORB_ID(input_rc));
int servo_fd, result;
servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
servo_position_t pos;
servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);

1
src/systemcmds/tests/test_rc.c
View File

@ -52,6 +52,7 @@
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include "tests.h"

1
src/systemcmds/tests/test_servo.c
View File

@ -51,6 +51,7 @@
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/err.h>
#include <nuttx/spi.h>