Discussions

Hello,

I have been trying to use collision avoidance in Arducopter 2.91 using the old ArducopterNG code for the algorithm of obstacle avoidance. I have defined the Ultrasonic sensors using the HAL subsystem and am using the analog channels being used for the motor LEDs. The folowing are the changes/additions that I have made.  The issue is that when I upload this code, the radio subsystem stops working. If I turn off the transmitter I get random values, but, when the Tx is on, there is no sign of life.

In Arducopter.pde

*******************************

ModeFilterInt16_Size3 fr_mode_filter(1);
ModeFilterInt16_Size3 br_mode_filter(1);
ModeFilterInt16_Size3 lr_mode_filter(1);
ModeFilterInt16_Size3 rr_mode_filter(1);
//ModeFilterInt16_Size5 mode_filter(4);
#if CA_SONAR == ENABLED
AP_HAL::AnalogSource *sonar_f_source;
AP_HAL::AnalogSource *sonar_b_source;
AP_HAL::AnalogSource *sonar_l_source;
AP_HAL::AnalogSource *sonar_r_source;

AP_RangeFinder_MaxsonarXL *front_range;
AP_RangeFinder_MaxsonarXL *back_range;
AP_RangeFinder_MaxsonarXL *left_range;
AP_RangeFinder_MaxsonarXL *right_range;

sonar_f_source = hal.analogin->channel(SONAR_F_SOURCE);
front_range = new AP_RangeFinder_MaxsonarXL(sonar_f_source, &fr_mode_filter);

sonar_b_source = hal.analogin->channel(SONAR_B_SOURCE);
back_range = new AP_RangeFinder_MaxsonarXL(sonar_b_source, &br_mode_filter);

sonar_l_source = hal.analogin->channel(SONAR_L_SOURCE);
left_range = new AP_RangeFinder_MaxsonarXL(sonar_l_source, &lr_mode_filter);

sonar_r_source = hal.analogin->channel(SONAR_R_SOURCE);
right_range = new AP_RangeFinder_MaxsonarXL(sonar_r_source, &rr_mode_filter);

#endif

*************************************

In config.h

**************************************

#define COPTER_LEDS DISABLED
// #define COPTER_LEDS ENABLED

**************************************

In defines.h

**************************************

# define CA_SONAR DISABLED

//Assign channels for the 4 CA sensors
#define SONAR_F_SOURCE 4 //Front
#define SONAR_L_SOURCE 5 //Back
#define SONAR_B_SOURCE 6 //Left
#define SONAR_R_SOURCE 7 //Right

//define the orientation of the sensors w.r.t. the center of the quad.
//Assuming the sensors will be mounted in '+' config even if Quad is in 'x' config
#define AP_RANGEFINDER_ORIENTATION_FRONT 0, 5, 0
#define AP_RANGEFINDER_ORIENTATION_RIGHT -5, 0, 0
#define AP_RANGEFINDER_ORIENTATION_BACK 0, -5, 0
#define AP_RANGEFINDER_ORIENTATION_LEFT 5, 0, 0

//Some variables for use in the obstacle avaoidance function
float CA_roll_I=0;
float CA_pitch_I=0;
float CA_throttle_I=0;
float command_CA_roll = 0;
float command_CA_pitch = 0;
float command_CA_throttle = 0;
char CA_new_data = 0;

//PID performance parameters
float KP_CA_ROLL = 10;
float KD_CA_ROLL = 0.1;
float KI_CA_ROLL = 0.1;
float KP_CA_PITCH = 10;
float KD_CA_PITCH = 0.1;
float KI_CA_PITCH = 0.1;
float CA_MAX_ANGLE = 20; // Maximun command roll and pitch angle from obstacle avoiding control
float CA_SAFETY_ZONE = 30; // object avoidance will move away from objects within this distance (in cm)

*********************************************

in Parameters.h

*****************************************

enum{ ..............

k_param_ca_sonar,

..................}

AP_Int8         ca_sonar;

*****************************************

in Parameters.pde

*****************************************

const AP_Param::Info var_info[] PROGMEM = { ..................

GSCALAR(ca_sonar, "CA_SONAR_ENABLE", CA_SONAR),

...................}

******************************************

in Sensors.pde

******************************************

static void init_ca_sonar(void)
{ float scaling = 5;
#if CA_SONAR == ENABLED
front_range->set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT);
back_range->set_orientation(AP_RANGEFINDER_ORIENTATION_BACK);
left_range->set_orientation(AP_RANGEFINDER_ORIENTATION_LEFT);
right_range->set_orientation(AP_RANGEFINDER_ORIENTATION_RIGHT);

front_range->calculate_scaler(0, scaling); // setup ADC ref vtg for Front CA sonar //SONAR_TYPE=0 for MaxsonarXL
back_range->calculate_scaler(0, scaling); // setup ADC ref vtg for Back CA sonar //SONAR_TYPE=0 for MaxsonarXL
left_range->calculate_scaler(0, scaling); // setup ADC ref vtg for Left CA sonar //SONAR_TYPE=0 for MaxsonarXL
right_range->calculate_scaler(0, scaling); // setup ADC ref vtg for Right CA sonar //SONAR_TYPE=0 for MaxsonarXL
#endif
}

******************************************

in system.pde

*****************************************

#if CA_SONAR == ENABLED
init_ca_sonar();
#endif

*******************************************

in setup.pde

*******************************************

static int8_t   setup_ca_sonar          (uint8_t argc, const Menu::arg *argv);

const struct Menu::command setup_menu_commands[] PROGMEM = { ..........

{"ca_sonar",                    setup_ca_sonar}, ............}

static int8_t
setup_ca_sonar(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.ca_sonar.set_and_save(true);

} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.ca_sonar.set_and_save(false);

}else{
cliSerial->printf_P(PSTR("\nOp:[on, off, 0-3]\n"));
report_ca_sonar();
return 0;
}

report_ca_sonar();
return 0;
}

static void report_ca_sonar()
{
cliSerial->printf_P(PSTR("CA_Sonar\n"));
print_divider();
print_enabled(g.ca_sonar.get());
cliSerial->printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type);
print_blanks(2);
}

*******************************

in usercode.pde

*******************************

void userhook_50Hz()
{
#if CA_SONAR == ENABLED

if( CA_new_data ) 
{
Obstacle_avoidance(CA_SAFETY_ZONE); //CA function. Takes the CA distance as arg.
CA_new_data = 0; // record that we have consumed the rangefinder data
}
#endif

}

*********************************

Please help me in isolating & solving the issue.

Thanks,

Swaroop