mirror of https://github.com/ArduPilot/ardupilot
122 lines
3.1 KiB
C++
122 lines
3.1 KiB
C++
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <StorageManager/StorageManager.h>
|
|
#include <AP_Progmem/AP_Progmem.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_HAL_AVR/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();
|
|
|