void sensors_begin(bool flipped) {
hal.console->printf("init AP_InertialSensor: ");
if (flipped) {
g_ahrs.set_orientation(ROTATION_ROLL_180);
}
g_ins.init(AP_InertialSensor::COLD_START, INS_SAMPLE_RATE, flash_leds);
g_ins.init_accel(flash_leds);
hal.console->println();
led_set(0, false);
hal.console->printf("done\r\n");
hal.console->printf("init AP_Compass: "******"done\r\n");
hal.console->printf("init AP_Baro: ");
g_baro.init();
g_baro.calibrate();
hal.console->printf("done\r\n");
hal.console->printf("init AP_AHRS: ");
g_ahrs.init();
g_ahrs.set_compass(&g_compass);
hal.console->printf("sensors init done\r\n");
}
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
void setup()
{
// Enable the motors and set at 490Hz update
hal.rcout->set_freq(0xF, 490);
hal.rcout->enable_mask(0xFF);
// PID Configuration
pids[PID_PITCH_RATE].kP(0.7);
pids[PID_PITCH_RATE].kI(1);
pids[PID_PITCH_RATE].imax(50);
pids[PID_ROLL_RATE].kP(0.7);
pids[PID_ROLL_RATE].kI(1);
pids[PID_ROLL_RATE].imax(50);
pids[PID_YAW_RATE].kP(2.7);
pids[PID_YAW_RATE].kI(1);
pids[PID_YAW_RATE].imax(50);
pids[PID_PITCH_STAB].kP(4.5);
pids[PID_ROLL_STAB].kP(4.5);
pids[PID_YAW_STAB].kP(10);
// Turn off Barometer to avoid bus collisions
hal.gpio->pinMode(40, GPIO_OUTPUT);
hal.gpio->write(40, 1);
// Turn on MPU6050 - quad must be kept still as gyros will calibrate
ins.init(AP_InertialSensor::COLD_START,
AP_InertialSensor::RATE_100HZ,
NULL);
// initialise sensor fusion on MPU6050 chip (aka DigitalMotionProcessing/DMP)
hal.scheduler->suspend_timer_procs(); // stop bus collisions
ins.dmp_init();
hal.scheduler->resume_timer_procs();
hal.uartA->begin(57600); // for radios
// We're ready to go! Now over to loop()
}
static void sensors_debug() {
static int divider = 0;
if (divider++ == 20) {
divider = 0;
float heading = g_compass.calculate_heading(g_ahrs.get_dcm_matrix());
hal.console->printf("ahrs: roll %4.1f pitch %4.1f "
"yaw %4.1f hdg %.1f\r\n",
ToDeg(g_ahrs.roll), ToDeg(g_ahrs.pitch),
ToDeg(g_ahrs.yaw),
g_compass.use_for_yaw() ? ToDeg(heading):0.0f);
Vector3f accel(g_ins.get_accel());
Vector3f gyro(g_ins.get_gyro());
hal.console->printf("mpu6000: accel %.2f %.2f %.2f "
"gyro %.2f %.2f %.2f\r\n",
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z);
hal.console->printf("compass: heading %.2f deg\r\n",
ToDeg(g_compass.calculate_heading(0, 0)));
}
}
void main_task(void *args)
{
vTaskSetApplicationTaskTag(xTaskGetIdleTaskHandle(), (pdTASK_HOOK_CODE)1);
vTaskSetApplicationTaskTag(NULL, (pdTASK_HOOK_CODE)2);
led_init();
hal.init(0, NULL);
hal.console->printf("AP_HAL Sensor Test\r\n");
hal.scheduler->register_timer_failsafe(failsafe, 1000);
hal.console->printf("init AP_InertialSensor: ");
g_ins.init(AP_InertialSensor::COLD_START, INS_SAMPLE_RATE, flash_leds);
g_ins.init_accel(flash_leds);
hal.console->println();
led_set(0, false);
hal.console->printf("done\r\n");
hal.console->printf("init AP_Compass: "******"done\r\n");
hal.console->printf("init AP_Baro: ");
g_baro.init();
g_baro.calibrate();
hal.console->printf("done\r\n");
hal.console->printf("init AP_AHRS: ");
g_ahrs.init();
g_ahrs.set_compass(&g_compass);
g_ahrs.set_barometer(&g_baro);
hal.console->printf("done\r\n");
portTickType last_print = 0;
portTickType last_compass = 0;
portTickType last_wake = 0;
float heading = 0.0f;
last_wake = xTaskGetTickCount();
for (;;) {
// Delay to run this loop at 100Hz.
vTaskDelayUntil(&last_wake, 10);
portTickType now = xTaskGetTickCount();
if (last_compass == 0 || now - last_compass > 100) {
last_compass = now;
g_compass.read();
g_baro.read();
heading = g_compass.calculate_heading(g_ahrs.get_dcm_matrix());
}
g_ahrs.update();
if (last_print == 0 || now - last_print > 100) {
last_print = now;
hal.console->write("\r\n");
hal.console->printf("ahrs: roll %4.1f pitch %4.1f yaw %4.1f hdg %.1f\r\n",
ToDeg(g_ahrs.roll), ToDeg(g_ahrs.pitch),
ToDeg(g_ahrs.yaw),
g_compass.use_for_yaw() ? ToDeg(heading) : 0.0f);
Vector3f accel(g_ins.get_accel());
Vector3f gyro(g_ins.get_gyro());
hal.console->printf("mpu6000: accel %.2f %.2f %.2f "
"gyro %.2f %.2f %.2f\r\n",
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z);
hal.console->printf("compass: heading %.2f deg\r\n",
ToDeg(g_compass.calculate_heading(0, 0)));
g_compass.null_offsets();
if (hal.rcin->valid()) {
uint16_t ppm[PPM_MAX_CHANNELS];
size_t count;
count = hal.rcin->read(ppm, PPM_MAX_CHANNELS);
hal.console->write("ppm: ");
for (size_t i = 0; i < count; ++i)
hal.console->printf("%u ", ppm[i]);
hal.console->write("\r\n");
}
}
}
}
void loop()
{
static float yaw_target = 0;
// Wait until new orientation data (normally 5ms max)
while (ins.num_samples_available() == 0);
static uint32_t lastPkt = 0;
static int16_t channels[8] = {0,0,0,0,0,0,0,0};
Iriss::Command doSomething = check_for_command();
do_respond(doSomething, channels);
// turn throttle off if no update for 0.5seconds
if(hal.scheduler->millis() - lastPkt > 500) {
channels[2] = 0;
}
long rcthr, rcyaw, rcpit, rcroll; // Variables to store radio in
// Read RC transmitter
rcthr = channels[2];
rcyaw = channels[3];
rcpit = channels[1];
rcroll = channels[0];
// Ask MPU6050 for orientation
ins.update();
float roll,pitch,yaw;
ins.quaternion.to_euler(&roll, &pitch, &yaw);
roll = ToDeg(roll) ;
pitch = ToDeg(pitch) ;
yaw = ToDeg(yaw) ;
// Ask MPU6050 for gyro data
Vector3f gyro = ins.get_gyro();
float gyroPitch = ToDeg(gyro.y), gyroRoll = ToDeg(gyro.x), gyroYaw = ToDeg(gyro.z);
// Do the magic
if(rcthr > RC_THR_MIN + 100) { // Throttle raised, turn on stablisation.
// Stablise PIDS
float pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250);
float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);
float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);
// is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
if(abs(rcyaw ) > 5) {
yaw_stab_output = rcyaw;
yaw_target = yaw; // remember this yaw for when pilot stops
}
// rate PIDS
long pitch_output = (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500);
long roll_output = (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);
long yaw_output = (long) constrain(pids[PID_ROLL_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);
// mix pid outputs and send to the motors.
hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output);
hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output);
hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output);
hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output);
}
else {
// motors off
hal.rcout->write(MOTOR_FL, 1000);
hal.rcout->write(MOTOR_BL, 1000);
hal.rcout->write(MOTOR_FR, 1000);
hal.rcout->write(MOTOR_BR, 1000);
// reset yaw target so we maintain this on takeoff
yaw_target = yaw;
// reset PID integrals whilst on the ground
for(int i=0; i<6; i++) {
pids[i].reset_I();
}
}
}
void do_respond(const Iriss::Command& cmd, int16_t *channels)
{
uint32_t directives = cmd.get();
if(directives == Iriss::Command::TX_ERR) {
return;
}
if(directives & Iriss::Command::ACK) {
// reply with an ACK if not waiting on one
if(!piOnline) {
Iriss::Command resp(Iriss::Command::ACK);
hal.console->write(&Iriss::BEGIN_UART_MESSAGE, 1);
uint8_t lenBuf[4];
Utils::Packable::pack_uint32(Iriss::Command::PACKED_SIZE, lenBuf);
hal.console->write(lenBuf, sizeof(Iriss::Command::PACKED_SIZE));
uint8_t cmdBuf[Iriss::Command::PACKED_SIZE];
resp.pack(cmdBuf, Iriss::Command::PACKED_SIZE);
hal.console->write(cmdBuf, Iriss::Command::PACKED_SIZE);
piOnline = true;
}
}
if(directives & Iriss::Command::SEND_AGAIN) {
// send the last command
Iriss::Command resp;
resp.set(lastSent);
hal.console->write(&Iriss::BEGIN_UART_MESSAGE, 1);
uint8_t lenBuf[4];
Utils::Packable::pack_uint32(Iriss::Command::PACKED_SIZE, lenBuf);
hal.console->write(lenBuf, sizeof(Iriss::Command::PACKED_SIZE));
uint8_t cmdBuf[Iriss::Command::PACKED_SIZE];
resp.pack(cmdBuf, Iriss::Command::PACKED_SIZE);
hal.console->write(cmdBuf, Iriss::Command::PACKED_SIZE);
}
if(directives & Iriss::Command::GET_ORIENTATION) {
// get the orientation from ins and send it
ins.update();
float roll,pitch,yaw;
ins.quaternion.to_euler(&roll, &pitch, &yaw);
roll = ToDeg(roll);
pitch = ToDeg(pitch);
yaw = ToDeg(yaw);
Iriss::Orientation orientation(roll, pitch, yaw);
hal.console->write(&Iriss::BEGIN_UART_MESSAGE, 1);
uint8_t lenBuf[4];
Utils::Packable::pack_uint32(Iriss::Orientation::PACKED_SIZE, lenBuf);
hal.console->write(lenBuf, sizeof(Iriss::Command::PACKED_SIZE));
uint8_t orientBuf[Iriss::Orientation::PACKED_SIZE];
orientation.pack(orientBuf, Iriss::Orientation::PACKED_SIZE);
hal.console->write(orientBuf, Iriss::Orientation::PACKED_SIZE);
}
if(directives & Iriss::Command::NUDGE_ROLL_LEFT) {
// update channels 0
--channels[0];
}
if(directives & Iriss::Command::NUDGE_ROLL_RIGHT) {
// update channels 0 in the other way from LEFT
++channels[0];
}
if(directives & Iriss::Command::NUDGE_UP) {
// update channels 2
++channels[2];
}
if(directives & Iriss::Command::NUDGE_DOWN) {
// update channels 2 in the other way from UP
--channels[2];
}
if(directives & Iriss::Command::NUDGE_YAW_CCW) {
// update channels 3
++channels[3];
}
if(directives & Iriss::Command::NUDGE_YAW_CW) {
// update channels 3 in the other way from CCW
--channels[3];
}
if(directives & Iriss::Command::NUDGE_PITCH_DOWN) {
// update channels 1
++channels[1];
}
if(directives & Iriss::Command::NUDGE_PITCH_UP) {
// update channels 1 in the other way from DOWN
--channels[1];
}
}
