mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: sketch cpp builds!
This commit is contained in:
Pat Hickey
Andrew Tridgell
parent
8b49208771
commit
5bd7046a95
|
|
@ -844,6 +844,13 @@ AP_Relay relay;
|
|||
// is to take the 0 to 1024 range down to an 8 bit range for MAVLink
|
||||
AP_HAL::AnalogSource* rssi_analog_source;
|
||||
|
||||
|
||||
// Input sources for battery voltage, battery current, board vcc
|
||||
AP_HAL::AnalogSource* batt_volt_analog_source;
|
||||
AP_HAL::AnalogSource* batt_curr_analog_source;
|
||||
AP_HAL::AnalogSource* board_vcc_analog_source;
|
||||
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
||||
#endif
|
||||
|
|
@ -902,6 +909,9 @@ void setup() {
|
|||
#endif
|
||||
|
||||
rssi_analog_source = hal.analogin->channel(g.rssi_pin, 0.25);
|
||||
batt_volt_analog_source = hal.analogin->channel(g.battery_volt_pin);
|
||||
batt_curr_analog_source = hal.analogin->channel(g.battery_curr_pin);
|
||||
board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
|
||||
|
||||
memcheck_init();
|
||||
init_ardupilot();
|
||||
|
|
|
|||
|
|
@ -82,14 +82,12 @@ static void read_battery(void)
|
|||
}
|
||||
|
||||
if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
|
||||
static AP_AnalogSource_Arduino batt_volt_pin(g.battery_volt_pin);
|
||||
batt_volt_pin.set_pin(g.battery_volt_pin);
|
||||
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin.read_average());
|
||||
batt_volt_analog_source->set_pin(g.battery_volt_pin);
|
||||
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_analog_source->read_average());
|
||||
}
|
||||
if(g.battery_monitoring == 4) {
|
||||
static AP_AnalogSource_Arduino batt_curr_pin(g.battery_curr_pin);
|
||||
batt_curr_pin.set_pin(g.battery_curr_pin);
|
||||
current_amps1 = CURRENT_AMPS(batt_curr_pin.read_average());
|
||||
batt_curr_analog_source->set_pin(g.battery_curr_pin);
|
||||
current_amps1 = CURRENT_AMPS(batt_curr_analog_source->read_average());
|
||||
current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours)
|
||||
}
|
||||
|
||||
|
|
@ -111,6 +109,6 @@ static void read_battery(void)
|
|||
void read_receiver_rssi(void)
|
||||
{
|
||||
rssi_analog_source->set_pin(g.rssi_pin);
|
||||
float ret = rssi_analog_source->read();
|
||||
float ret = rssi_analog_source->read_latest();
|
||||
receiver_rssi = constrain(ret, 0, 255);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -207,7 +207,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
if(cliSerial->available() > 0) {
|
||||
delay(20);
|
||||
cliSerial->flush();
|
||||
while (cliSerial->read() != -1); /* flush */
|
||||
|
||||
g.rc_1.save_eeprom();
|
||||
g.rc_2.save_eeprom();
|
||||
|
|
@ -249,8 +249,8 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
ins.init(AP_InertialSensor::COLD_START,
|
||||
ins_sample_rate,
|
||||
delay, flash_leds, &timer_scheduler);
|
||||
ins.init_accel(delay, flash_leds);
|
||||
flash_leds);
|
||||
ins.init_accel(flash_leds);
|
||||
report_ins();
|
||||
return(0);
|
||||
}
|
||||
|
|
@ -263,7 +263,7 @@ static void setup_printf_P(const prog_char_t *fmt, ...)
|
|||
{
|
||||
va_list arg_list;
|
||||
va_start(arg_list, fmt);
|
||||
cliSerial->vprintf_P(fmt, arg_list);
|
||||
cliSerial->printf_P(fmt, arg_list);
|
||||
va_end(arg_list);
|
||||
}
|
||||
|
||||
|
|
@ -286,8 +286,8 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv)
|
|||
cliSerial->println_P(PSTR("Initialising gyros"));
|
||||
ins.init(AP_InertialSensor::COLD_START,
|
||||
ins_sample_rate,
|
||||
delay, flash_leds, &timer_scheduler);
|
||||
ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key);
|
||||
flash_leds);
|
||||
ins.calibrate_accel(flash_leds, cliSerial);
|
||||
report_ins();
|
||||
return(0);
|
||||
}
|
||||
|
|
@ -986,12 +986,10 @@ print_done()
|
|||
|
||||
static void zero_eeprom(void)
|
||||
{
|
||||
uint8_t b = 0;
|
||||
|
||||
cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
|
||||
|
||||
for (uintptr_t i = 0; i < EEPROM_MAX_ADDR; i++) {
|
||||
eeprom_write_byte((uint8_t *) i, b);
|
||||
for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i++) {
|
||||
hal.storage->write_byte(i, 0);
|
||||
}
|
||||
|
||||
cliSerial->printf_P(PSTR("done\n"));
|
||||
|
|
@ -1073,7 +1071,7 @@ static bool
|
|||
wait_for_yes()
|
||||
{
|
||||
int c;
|
||||
cliSerial->flush();
|
||||
while (cliSerial->read() != -1); /* flush */
|
||||
cliSerial->printf_P(PSTR("Y to save\n"));
|
||||
|
||||
do {
|
||||
|
|
|
|||
|
|
@ -86,42 +86,20 @@ static void init_ardupilot()
|
|||
// The console port buffers are defined to be sufficiently large to support
|
||||
// the MAVLink protocol efficiently
|
||||
//
|
||||
cliSerial->begin(SERIAL0_BAUD, 128, 256);
|
||||
hal.uartA->begin(SERIAL0_BAUD, 128, 256);
|
||||
|
||||
// GPS serial port.
|
||||
//
|
||||
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
|
||||
// standard gps running. Note that we need a 256 byte buffer for some
|
||||
// GPS types (eg. UBLOX)
|
||||
Serial1.begin(38400, 256, 16);
|
||||
hal.uartB->begin(38400, 256, 16);
|
||||
#endif
|
||||
|
||||
cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
|
||||
"\n\nFree RAM: %u\n"),
|
||||
memcheck_available_memory());
|
||||
|
||||
//
|
||||
// Initialize Wire and SPI libraries
|
||||
//
|
||||
#ifndef DESKTOP_BUILD
|
||||
I2c.begin();
|
||||
I2c.timeOut(5);
|
||||
// initially set a fast I2c speed, and drop it on first failures
|
||||
I2c.setSpeed(true);
|
||||
#endif
|
||||
SPI.begin();
|
||||
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
|
||||
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
SPI3.begin();
|
||||
SPI3.setSpeed(SPI3_SPEED_2MHZ);
|
||||
#endif
|
||||
|
||||
//
|
||||
// Initialize the isr_registry.
|
||||
//
|
||||
isr_registry.init();
|
||||
|
||||
//
|
||||
// Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function)
|
||||
//
|
||||
|
|
@ -143,8 +121,9 @@ static void init_ardupilot()
|
|||
#if CONFIG_PUSHBUTTON == ENABLED
|
||||
pinMode(PUSHBUTTON_PIN, INPUT); // unused
|
||||
#endif
|
||||
|
||||
#if CONFIG_RELAY == ENABLED
|
||||
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
|
||||
relay.init();
|
||||
#endif
|
||||
|
||||
#if COPTER_LEDS == ENABLED
|
||||
|
|
@ -168,7 +147,7 @@ static void init_ardupilot()
|
|||
load_parameters();
|
||||
|
||||
// init the GCS
|
||||
gcs0.init(&Serial);
|
||||
gcs0.init(hal.uartA);
|
||||
|
||||
#if USB_MUX_PIN > 0
|
||||
if (!ap_system.usb_connected) {
|
||||
|
|
@ -178,8 +157,8 @@ static void init_ardupilot()
|
|||
}
|
||||
#else
|
||||
// we have a 2nd serial port for telemetry
|
||||
Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 256);
|
||||
gcs3.init(&Serial3);
|
||||
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 256);
|
||||
gcs3.init(hal.uartC);
|
||||
#endif
|
||||
|
||||
// identify ourselves correctly with the ground station
|
||||
|
|
@ -200,42 +179,26 @@ static void init_ardupilot()
|
|||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
#ifdef RADIO_OVERRIDE_DEFAULTS
|
||||
{
|
||||
int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
|
||||
APM_RC.setHIL(rc_override);
|
||||
}
|
||||
#endif
|
||||
*/
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
motors.servo_manual = false;
|
||||
motors.init_swash(); // heli initialisation
|
||||
#endif
|
||||
|
||||
RC_Channel::set_apm_rc(&APM_RC);
|
||||
init_rc_in(); // sets up rc channels from radio
|
||||
init_rc_out(); // sets up the timer libs
|
||||
|
||||
timer_scheduler.init( &isr_registry );
|
||||
|
||||
/*
|
||||
* setup the 'main loop is dead' check. Note that this relies on
|
||||
* the RC library being initialised.
|
||||
*/
|
||||
timer_scheduler.set_failsafe(failsafe_check);
|
||||
|
||||
// initialise the analog port reader
|
||||
AP_AnalogSource_Arduino::init_timer(&timer_scheduler);
|
||||
hal.scheduler->register_timer_failsafe(failsafe_check, 1000);
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if CONFIG_ADC == ENABLED
|
||||
// begin filtering the ADC Gyros
|
||||
adc.Init(&timer_scheduler); // APM ADC library initialization
|
||||
adc.Init(); // APM ADC library initialization
|
||||
#endif // CONFIG_ADC
|
||||
|
||||
barometer.init(&timer_scheduler);
|
||||
barometer.init();
|
||||
|
||||
#endif // HIL_MODE
|
||||
|
||||
|
|
@ -277,7 +240,7 @@ static void init_ardupilot()
|
|||
const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
|
||||
cliSerial->println_P(msg);
|
||||
#if USB_MUX_PIN == 0
|
||||
Serial3.println_P(msg);
|
||||
hal.uartC->println_P(msg);
|
||||
#endif
|
||||
#endif // CLI_ENABLED
|
||||
|
||||
|
|
@ -308,25 +271,29 @@ init_rate_controllers();
|
|||
|
||||
startup_ground();
|
||||
|
||||
// now that initialisation of IMU has occurred increase SPI to 2MHz
|
||||
SPI.setClockDivider(SPI_CLOCK_DIV8); // 2MHZ SPI rate
|
||||
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
Log_Write_Startup();
|
||||
#endif
|
||||
|
||||
init_ap_limits();
|
||||
|
||||
cliSerial->print_P(PSTR("\nReady to FLY "));
|
||||
}
|
||||
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters
|
||||
// Experimental AP_Limits library - set constraints, limits, fences, minima,
|
||||
// maxima on various parameters
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
static void init_ap_limits() {
|
||||
#ifdef AP_LIMITS
|
||||
// AP_Limits modules are stored as a _linked list_. That allows us to
|
||||
// define an infinite number of modules and also to allocate no space until
|
||||
// we actually need to.
|
||||
|
||||
// AP_Limits modules are stored as a _linked list_. That allows us to define an infinite number of modules
|
||||
// and also to allocate no space until we actually need to.
|
||||
|
||||
// The linked list looks (logically) like this
|
||||
// [limits module] -> [first limit module] -> [second limit module] -> [third limit module] -> NULL
|
||||
// The linked list looks (logically) like this [limits module] -> [first
|
||||
// limit module] -> [second limit module] -> [third limit module] -> NULL
|
||||
|
||||
|
||||
// The details of the linked list are handled by the methods
|
||||
|
|
@ -347,16 +314,13 @@ init_rate_controllers();
|
|||
m = limits.modules_next();
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
cliSerial->print_P(PSTR("\nReady to FLY "));
|
||||
}
|
||||
|
||||
|
||||
//********************************************************************************
|
||||
//******************************************************************************
|
||||
//This function does all the calibrations, etc. that we need during a ground start
|
||||
//********************************************************************************
|
||||
//******************************************************************************
|
||||
static void startup_ground(void)
|
||||
{
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
|
||||
|
|
@ -365,13 +329,13 @@ static void startup_ground(void)
|
|||
// -----------------------------
|
||||
ins.init(AP_InertialSensor::COLD_START,
|
||||
ins_sample_rate,
|
||||
mavlink_delay, flash_leds, &timer_scheduler);
|
||||
flash_leds);
|
||||
#if CLI_ENABLED == ENABLED
|
||||
report_ins();
|
||||
#endif
|
||||
|
||||
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
|
||||
ahrs.init(&timer_scheduler);
|
||||
ahrs.init();
|
||||
|
||||
// setup fast AHRS gains to get right attitude
|
||||
ahrs.set_fast_gains(true);
|
||||
|
|
@ -662,8 +626,7 @@ void flash_leds(bool on)
|
|||
*/
|
||||
uint16_t board_voltage(void)
|
||||
{
|
||||
static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC);
|
||||
return vcc.read_vcc();
|
||||
return board_vcc_analog_source->read_latest();
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
|
|||
//static int8_t test_mission(uint8_t argc, const Menu::arg *argv);
|
||||
|
||||
// this is declared here to remove compiler errors
|
||||
extern void print_latlon(BetterStream *s, int32_t lat_or_lon); // in Log.pde
|
||||
extern void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon); // in Log.pde
|
||||
|
||||
// This is the help function
|
||||
// PSTR is an AVR macro to read strings from flash memory
|
||||
|
|
@ -104,13 +104,14 @@ test_mode(uint8_t argc, const Menu::arg *argv)
|
|||
static int8_t
|
||||
test_eedump(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
uintptr_t i, j;
|
||||
|
||||
// hexdump the EEPROM
|
||||
for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
|
||||
for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i += 16) {
|
||||
cliSerial->printf_P(PSTR("%04x:"), i);
|
||||
for (j = 0; j < 16; j++)
|
||||
cliSerial->printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
|
||||
for (uint16_t j = 0; j < 16; j++) {
|
||||
int b = hal.storage->read_byte(i+j);
|
||||
cliSerial->printf_P(PSTR(" %02x"), b);
|
||||
}
|
||||
cliSerial->println();
|
||||
}
|
||||
return(0);
|
||||
|
|
@ -462,7 +463,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
ins.init(AP_InertialSensor::COLD_START,
|
||||
ins_sample_rate,
|
||||
delay, flash_leds, &timer_scheduler);
|
||||
flash_leds);
|
||||
|
||||
delay(50);
|
||||
|
||||
|
|
@ -509,9 +510,9 @@ test_gps(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
if (g_gps->new_data) {
|
||||
cliSerial->printf_P(PSTR("Lat: "));
|
||||
print_latlon(&Serial, g_gps->latitude);
|
||||
print_latlon(cliSerial, g_gps->latitude);
|
||||
cliSerial->printf_P(PSTR(", Lon "));
|
||||
print_latlon(&Serial, g_gps->longitude);
|
||||
print_latlon(cliSerial, g_gps->longitude);
|
||||
cliSerial->printf_P(PSTR(", Alt: %ldm, #sats: %d\n"),
|
||||
g_gps->altitude/100,
|
||||
g_gps->num_sats);
|
||||
|
|
@ -665,11 +666,11 @@ test_battery(uint8_t argc, const Menu::arg *argv)
|
|||
return (0);
|
||||
#else
|
||||
cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n"));
|
||||
cliSerial->flush();
|
||||
while(!cliSerial->available()) {
|
||||
while (cliSerial->read() != -1); /* flush */
|
||||
while(!cliSerial->available()) { /* wait for input */
|
||||
delay(100);
|
||||
}
|
||||
cliSerial->flush();
|
||||
while (cliSerial->read() != -1); /* flush */
|
||||
print_hit_enter();
|
||||
|
||||
// allow motors to spin
|
||||
|
|
@ -921,7 +922,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
|
|||
while(1) {
|
||||
delay(100);
|
||||
|
||||
cliSerial->printf_P(PSTR("Sonar: %d cm\n"), sonar.read());
|
||||
cliSerial->printf_P(PSTR("Sonar: %d cm\n"), sonar->read());
|
||||
//cliSerial->printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value);
|
||||
|
||||
if(cliSerial->available() > 0) {
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Toy Mode - THOR
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
static boolean CH7_toy_flag;
|
||||
static bool CH7_toy_flag;
|
||||
|
||||
#if TOY_MIXER == TOY_LOOKUP_TABLE
|
||||
static const int16_t toy_lookup[] = {
|
||||
|
|
|
|||
Loading…
Reference in New Issue