oap-sonar.c

这个开发机器人项目源码

/*
 * $Id: oap-sonar.c,v 1.5 2005/02/19 14:39:09 dwalters Exp $
 * 
 * Copyright (C) 2003 Dafydd Walters <[e-mail]>
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#include "../../config_api/config_api.h"
#include "../include26/i2c-dev-26.h"

#define VERSION "1.0"
#define CONFIG_FILENAME "/etc/oap.conf"
#define GET_SONAR_READING_COMMAND_MIN 1     // SMBus: Block Read 
#define GET_NUM_SONARS_COMMAND 17           // SMBus: Read Byte Data 
#define GET_SONAR_POWER_STATE_COMMAND 18    // SMBus: Read Byte Data
#define SET_NUM_SONARS_COMMAND 100          // SMBus: Write Byte Data
#define SET_TIMER_ZERO_COMMAND 101          // SMBus: Send Byte
#define SET_SONAR_POWER_ON_COMMAND 102      // SMBus: Send Byte
#define SET_SONAR_POWER_OFF_COMMAND 103     // SMBus: Send Byte

static int device_address;
static char filename[100];
static int file;

static void read_config();
static int check_params(int argc, char* argv[]);
static void report_sonar(int file, int channel);
static void report_num_sonars(int file);
static void report_power(int file);
static void set_num_sonars(int file, int num_chans);
static void zero_timer(int file);
static void power_up(int file);
static void power_down(int file);
static void show_usage(void);

int main(int argc, char* argv[])
{
    if (!check_params(argc, argv)) {
        show_usage();
        exit(EXIT_FAILURE);
    }

    /* Load configuration settings */
    read_config();

    /* Open device */    
    if ((file = open(filename, O_RDWR)) < 0) {
        fprintf(stderr, "Unable to open device %s\n", filename);
        exit(EXIT_FAILURE);
    }
    
    /* Select slave device address */
    if (ioctl(file, I2C_SLAVE, device_address) < 0) {
        fprintf(stderr, "Unable to set slave device address to %d\n", 
                device_address);
        close(file);
        exit(EXIT_FAILURE);
    }

    if (strcmp(argv[1], "r") == 0) {
        /* Report latest sonar reading for channel */
        report_sonar(file, atoi(argv[2]));
    }
    else if (strcmp(argv[1], "n") == 0) {
        /* Report number of sonars */
        report_num_sonars(file);
    }
    else if (strcmp(argv[1], "p") == 0) {
        /* Report power state */
        report_power(file);
    }
    if (strcmp(argv[1], "s") == 0) {
        /* Set number of sonar channels */
        set_num_sonars(file, atoi(argv[2]));
    }
    else if (strcmp(argv[1], "z") == 0) {
        /* Zero 8-bit timer counter */
        zero_timer(file);
    }
    else if (strcmp(argv[1], "u") == 0) {
        /* Power up sonar array */
        power_up(file);
    }
    else if (strcmp(argv[1], "d") == 0) {
        /* Power up sonar array */
        power_down(file);
    }
    
    close(file);
    exit(EXIT_SUCCESS);
}

void show_usage(void)
{
    printf("\nOpen Automaton Project Sonar Array Module utility - "
           "(oap-sonar) version %s\nCopyright (C) 2003 Dafydd Walters\n\n"
           "oap-sonar comes with ABSOLUTYELY NO WARRANTY; for details "
           "see the\nGNU General Public License at http://www.gnu.org"
           "/licenses/gpl.html\n\n", VERSION);

    printf("Usage: oap-sonar <command>\n"
            "Commands:\n"
            "  r <sonar>  Report latest sonar reading for given sonar channel (1-16)\n"
            "  n          Report number of sonars (stored in EEPROM)\n"
            "  p          Report power state (active or low-power)\n"
            "  s <num>    Set number of sonars (store in EEPROM) - even number 2 to 16\n"
            "  z          Zero 8-bit timer counter.\n"
            "  u          Power up sonar array (activate)\n"
            "  d          Power down sonar array (go to low-power mode)\n"
            "\n");
}

void report_sonar(int file, int channel)
{
    __s32 result;
    __u8 values[32];
    int microseconds, timer_tick;
    
    /* Read sonar block */
    result = oap_i2c_smbus_read_block_data(file, GET_SONAR_READING_COMMAND_MIN + channel - 1, values);
    if (result != 3) {
        fprintf(stderr, "Error reading sonar\n");
        close(file);
        exit(EXIT_FAILURE);
    }

    microseconds = values[0] + (256 * values[1]);
    timer_tick = values[2];
        
    printf("Ping time for sonar channel %d: %d microseconds\n", channel, microseconds);
    printf("8-bit timer tick count: %d\n", timer_tick);
    
}

void report_num_sonars(int file)
{
    __s32 result;
    
    /* Read number of sonars */
    result = oap_i2c_smbus_read_byte_data(file, GET_NUM_SONARS_COMMAND);

    if (result < 0) {
        fprintf(stderr, "Error reading number of sonars\n");
        close(file);
        exit(EXIT_FAILURE);
    }

    printf("Number of sonars: %d\n", result);
}

void report_power(int file)
{
    __s32 result;
    
    /* Read power state */
    result = oap_i2c_smbus_read_byte_data(file, GET_SONAR_POWER_STATE_COMMAND);

    if (result < 0) {
        fprintf(stderr, "Error reading power state\n");
        close(file);
        exit(EXIT_FAILURE);
    }

    printf("Power state: %s\n", (result ? "Active" : "Inactive (low-power)"));
}

void set_num_sonars(int file, int num_chans)
{
    __s32 result;
    
    /* Set number of sonar channels */
    result = oap_i2c_smbus_write_byte_data(file, SET_NUM_SONARS_COMMAND, 
                                       (__u8)num_chans);

    if (result < 0) {
        fprintf(stderr, "Error setting number of sonars\n");
        close(file);
        exit(EXIT_FAILURE);
    }
}

void zero_timer(int file)
{
    __s32 result;
    
    /* Set timer count to zero */
    result = oap_i2c_smbus_write_byte(file, SET_TIMER_ZERO_COMMAND);

    if (result < 0) {
        fprintf(stderr, "Error setting timer count to zero\n");
        close(file);
        exit(EXIT_FAILURE);
    }
}

void power_up(int file)
{
    __s32 result;
    
    /* Activate sonar array */
    result = oap_i2c_smbus_write_byte(file, SET_SONAR_POWER_ON_COMMAND);

    if (result < 0) {
        fprintf(stderr, "Error activating sonar array\n");
        close(file);
        exit(EXIT_FAILURE);
    }
}

void power_down(int file)
{
    __s32 result;
    
    /* De-activate sonar array */
    result = oap_i2c_smbus_write_byte(file, SET_SONAR_POWER_OFF_COMMAND);

    if (result < 0) {
        fprintf(stderr, "Error de-activating sonar array\n");
        close(file);
        exit(EXIT_FAILURE);
    }
}

int check_params(int argc, char* argv[])
{
    if ((argc == 3) && (strcmp(argv[1], "r") == 0)) 
        return 1;
    
    if ((argc == 2) && (strcmp(argv[1], "n") == 0)) 
        return 1;
    
    if ((argc == 2) && (strcmp(argv[1], "p") == 0)) 
        return 1;
    
    if ((argc == 3) && (strcmp(argv[1], "s") == 0) && (atoi(argv[2]) >= 2)
        && (atoi(argv[2]) <= 16) && (atoi(argv[2]) % 2 == 0) ) 
        return 1;
    
    if ((argc == 2) && (strcmp(argv[1], "z") == 0)) 
        return 1;
    
    if ((argc == 2) && (strcmp(argv[1], "u") == 0)) 
        return 1;
    
    if ((argc == 2) && (strcmp(argv[1], "d") == 0)) 
        return 1;
    
    return 0;
}

void read_config()
{
    config_t conf;
    char s_device_address[4];

    /* Open configuration file */
    if (!config_open(&conf, CONFIG_FILENAME, C_READ)) {
        fprintf(stderr, "Unable to open configuration file %s for reading\n", 
                CONFIG_FILENAME);
        exit(EXIT_FAILURE);
    }

    /* Read configuration settings */
    if (!config_read(&conf, "I2C", "DeviceName", filename, 100)) {
        fprintf(stderr, 
        "Unable to read DeviceName from [I2C] section of configuration file\n");
        config_close(&conf);
        exit(EXIT_FAILURE);
    }
    
    if (!config_read(&conf, "Sonar Array Module", "I2CAddress", 
                s_device_address, 4)) {
        fprintf(stderr, 
        "Unable to read I2CAddress from [Sonar Array Module] section of\n" 
        "configuration file.\n");
        config_close(&conf);
        exit(EXIT_FAILURE);
    }
    device_address = atoi(s_device_address);

    /* Close configuration file */
    config_close(&conf);
}