You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
121 lines
3.0 KiB
121 lines
3.0 KiB
#include <AP_Common.h> |
|
#include <AP_Math.h> |
|
#include <AP_Param.h> |
|
#include <StorageManager.h> |
|
#include <AP_Progmem.h> |
|
|
|
#include <AP_HAL.h> |
|
#include <AP_HAL_AVR.h> |
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 |
|
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; |
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 |
|
const AP_HAL::HAL& hal = AP_HAL_AVR_APM1; |
|
#endif |
|
|
|
uint16_t ppm_center[ 8 ]; |
|
uint16_t ppm_min[ 8 ]; |
|
uint16_t ppm_max[ 8 ]; |
|
uint16_t ppm_delta[ 8 ]; |
|
uint16_t ppm_mark[ 8 ]; |
|
|
|
|
|
void setup (void) { |
|
hal.console->printf_P(PSTR("RC input jitter test - Do not move sticks:\n")); |
|
|
|
hal.scheduler->delay(2000); |
|
|
|
for( uint8_t rep = 0; rep < 10; rep++ ) |
|
{ |
|
for( uint8_t i = 0; i < 8; i++ ) |
|
{ |
|
uint16_t ppm_tmp = hal.rcin->read( i ); |
|
ppm_center[ i ] += ppm_tmp; |
|
} |
|
hal.scheduler->delay( 100 ); |
|
} |
|
|
|
for( uint8_t i = 0; i < 8; i++ ) |
|
{ |
|
uint16_t ppm_tmp = ppm_center[ i ] / 10; |
|
ppm_center[ i ] = ppm_tmp; |
|
ppm_min[ i ] = ppm_tmp; |
|
ppm_max[ i ] = ppm_tmp; |
|
ppm_delta[ i ] = 0; |
|
} |
|
|
|
} |
|
|
|
void loop (void) { |
|
|
|
|
|
static bool update = true; |
|
static uint32_t time_trigger; |
|
|
|
for( uint8_t i = 0; i < 8; i++ ) |
|
{ |
|
uint16_t ppm_tmp = hal.rcin->read( i ); |
|
|
|
// min? |
|
if( ppm_tmp < ppm_min[ i ] ) |
|
{ |
|
uint16_t delta_tmp = ppm_center[ i ] - ppm_tmp; |
|
if( delta_tmp > ppm_delta[ i ] ) ppm_delta[ i ] = delta_tmp; |
|
|
|
ppm_min[ i ] = ppm_tmp; |
|
ppm_mark[ i ] = ppm_tmp; |
|
|
|
update = true; |
|
} |
|
|
|
// max? |
|
if( ppm_tmp > ppm_max[ i ] ) |
|
{ |
|
uint16_t delta_tmp = ppm_tmp - ppm_center[ i ]; |
|
if( delta_tmp > ppm_delta[ i ] ) ppm_delta[ i ] = delta_tmp; |
|
|
|
ppm_max[ i ] = ppm_tmp; |
|
ppm_mark[ i ] = ppm_tmp; |
|
|
|
update = true; |
|
} |
|
|
|
} |
|
|
|
if( update && hal.scheduler->millis() > time_trigger ) |
|
{ |
|
update = false; |
|
time_trigger = hal.scheduler->millis() + 5000; |
|
|
|
uint32_t time = hal.scheduler->millis() / 1000; |
|
|
|
uint16_t hour = time / 3600; |
|
time -= hour * 3600; |
|
|
|
uint16_t minutte = time / 60; |
|
time -= minutte * 60; |
|
|
|
uint16_t second = time; |
|
|
|
//hal.console->printf_P( PSTR("%c%c%c\r\n"), 0x1B, 0x5B, 0x48 ); // Terminal home position |
|
hal.console->printf_P( PSTR("\n< %.2u:%.2u:%.2u >------------------------------------- \n"), hour, minutte, second ); |
|
for( uint8_t i = 0; i < 8; i++ ) |
|
{ |
|
hal.console->printf_P( PSTR("ch%d: center:%d min:%d max:%d delta:%d"), |
|
i + 1, ppm_center[ i ], ppm_min[ i ],ppm_max[ i ], ppm_delta[ i ] ); |
|
|
|
if( ppm_mark[ i ] > 0 ) |
|
{ |
|
hal.console->printf_P( PSTR(" [%d]"), ppm_mark[ i ] ); |
|
ppm_mark[ i ] = 0; |
|
} |
|
|
|
hal.console->printf_P( PSTR("\n") ); |
|
} |
|
} |
|
|
|
hal.scheduler->delay( 10 ); |
|
} |
|
|
|
AP_HAL_MAIN(); |
|
|
|
|